Compare commits
11 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 onur
|
dc5796e2fc | ||
 onur
|
50454f8c23 | ||
|
onur
|
347e1c671d | ||
|
onur
|
a6bb080270 | ||
|
onur
|
9b0828dbaa | ||
|
onur
|
a697284bff | ||
|
onur
|
988f9686e7 | ||
|
onur
|
73db2a8355 | ||
|
onur
|
156cfc8b77 | ||
|
onur
|
23650be635 | ||
|
onur
|
49b8609bb4 |
@@ -7,3 +7,9 @@
|
||||
# Generated build outputs.
|
||||
/out/
|
||||
/soc/out/
|
||||
/hardware/generated/
|
||||
|
||||
# Python runtime caches and privacy-sensitive raw board captures.
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
/soc/software/pps_end_to_end_validation/hardware_logs/*.csv
|
||||
|
||||
@@ -19,7 +19,7 @@ help:
|
||||
' make doctor # check local JDK/Mill/CIRCT and RISC-V tools' \
|
||||
' make soc-hw # emit shared SoC FIRRTL through FPGA config' \
|
||||
' make fpga-rtl # emit selected FPGA SystemVerilog and wrapper' \
|
||||
' make fpga-bitstream # run selected FPGA Vivado bitstream flow' \
|
||||
' make fpga-bitstream # run selected board vendor bitstream flow' \
|
||||
' make fpga-elf-flash-image' \
|
||||
' # build APP ELF and board flash-format app image' \
|
||||
' make fpga-mcs # generate combined bitstream + app MCS/BIN' \
|
||||
@@ -39,13 +39,17 @@ help:
|
||||
' make asic-sim # run ASIC-style Questa sanity simulation' \
|
||||
'' \
|
||||
'Config:' \
|
||||
' FPGA_BOARD=$(FPGA_BOARD) # vcu118 or genesys2' \
|
||||
' FPGA_BOARD=$(FPGA_BOARD) # vcu118, genesys2, or ti375c529' \
|
||||
' UART_BAUD=460800 # FPGA firmware and host listener default' \
|
||||
' GPS_ESKF_ENABLE=1 # gps_system: 1 ESKF, 0 GPS-only' \
|
||||
' IMU_SENSOR=auto # gps_system: auto, tdk, st or stm' \
|
||||
'' \
|
||||
'Direct entry points:' \
|
||||
' make -C soc help' \
|
||||
' make -C hardware/soc help' \
|
||||
' make -C targets/fpga/vcu118 help' \
|
||||
' make -C targets/fpga/genesys2 help' \
|
||||
' make -C targets/fpga/ti375c529 help' \
|
||||
' make -C targets/asic/rocket help'
|
||||
|
||||
doctor:
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
# Rocket Project
|
||||
|
||||
Bu agac VCU118/Genesys2 FPGA ve ASIC-style Rocket akislari icin sade proje duzenidir.
|
||||
Bu agac VCU118, Genesys2 ve Ti375C529 FPGA akislari ile ASIC-style Rocket
|
||||
akisi icin sade proje duzenidir.
|
||||
Upstream Rocket-Chip kaynaklari daginik kok klasorlerde tutulmaz; gerekli
|
||||
upstream kaynaklar `hardware/upstream/rocket-chip/` altindadir.
|
||||
|
||||
@@ -22,6 +23,7 @@ hardware/
|
||||
soc/software/ Bootloader, BSP, uygulamalar, flash image tool
|
||||
targets/fpga/vcu118/ VCU118 build/programlama akisi
|
||||
targets/fpga/genesys2/ Genesys2 build/programlama akisi
|
||||
targets/fpga/ti375c529/ Ti375C529 Efinity build/programlama akisi
|
||||
targets/asic/rocket/ ASIC wrapper/filelist/sim akisi
|
||||
setup_tools.sh /tools/soc-tools host araci kurulum betigi
|
||||
```
|
||||
@@ -86,6 +88,121 @@ make -C soc/software app APP=gps_system
|
||||
make -C targets/fpga/vcu118 debug-load APP=gps_system
|
||||
```
|
||||
|
||||
## GPS Firmware Profilleri: ESKF ve GPS-only
|
||||
|
||||
`gps_system` varsayilan olarak ESKF acik (`GPS_ESKF_ENABLE=1`) ve UART
|
||||
`460800` baud olacak sekilde derlenir. Kok Makefile'in kart varsayilani VCU118
|
||||
oldugu icin Genesys2 komutlarinda `FPGA_BOARD=genesys2` acikca yazilmalidir.
|
||||
|
||||
TDK ICM42688 + GPS + ESKF iceren birlesik Genesys2 MCS/BIN:
|
||||
|
||||
```sh
|
||||
make FPGA_BOARD=genesys2 APP=gps_system \
|
||||
GPS_ESKF_ENABLE=1 IMU_SENSOR=tdk fpga-mcs
|
||||
```
|
||||
|
||||
Yalniz GPS acquisition/tracking, WLS, Doppler velocity ve PPS calistiran;
|
||||
IMU/ESKF baslatmayan firmware:
|
||||
|
||||
```sh
|
||||
make FPGA_BOARD=genesys2 APP=gps_system \
|
||||
GPS_ESKF_ENABLE=0 fpga-mcs
|
||||
```
|
||||
|
||||
`GPS_ESKF_ENABLE` verilmezse deger `1`'dir. Iki komut ayni
|
||||
`targets/fpga/genesys2/out/flash/genesys2_rocket.mcs` ve `.bin` yollarini
|
||||
kullanir; ikinci build birincinin ciktisinin uzerine yazar. Iki profili
|
||||
saklamak gerekiyorsa her build'den sonra dosyayi profil adiyla kopyalayin.
|
||||
|
||||
`fpga-mcs` mevcut `.bit` dosyasini kullanip bootloader ve uygulamayi yeniden
|
||||
derler. Yalniz software profili veya baud degisikligi icin sentez gerekmez.
|
||||
Tracking/PPS RTL veya Chisel degistiyse once:
|
||||
|
||||
```sh
|
||||
make FPGA_BOARD=genesys2 APP=gps_system fpga-bitstream
|
||||
make FPGA_BOARD=genesys2 APP=gps_system GPS_ESKF_ENABLE=1 \
|
||||
IMU_SENSOR=tdk fpga-mcs
|
||||
```
|
||||
|
||||
Firmware ve host UART dinleyicisinin varsayilani birlikte `460800`'dur:
|
||||
|
||||
```sh
|
||||
make FPGA_BOARD=genesys2 fpga-uart
|
||||
```
|
||||
|
||||
### Doppler, WLS ve ESKF veri yolu
|
||||
|
||||
```text
|
||||
tracking loop, her 20 ms:
|
||||
carrier Q7 + 20 prompt I/Q + wideband power + ABS_SAMPLE/code phase
|
||||
-> sabit tracking snapshot
|
||||
-> synchronizer + APB-domain atomik register + interrupt
|
||||
-> C/N0 ve Doppler/range-rate donusumu
|
||||
-> ham Doppler + tek-kanal FDE ile C/N0-weighted velocity WLS
|
||||
-> ECEF velocity + 3x3 covariance + receiver clock drift
|
||||
-> ECEF->ENU adapter
|
||||
-> ayri velocity NIS gate
|
||||
-> ayni capture epoch'unda 15-state ESKF update
|
||||
-> daha yeni IMU history replay
|
||||
```
|
||||
|
||||
RTL Doppler veya C/N0 uzerinde IIR yapmaz; ham ve 20 ms sabit snapshot'i
|
||||
software'e verir. Software ayni PRN icin C/N0'yi `alpha=0.2` ile yumusatir;
|
||||
Doppler'in hem ham de `alpha=0.05` IIR sonucu korunur. L1 range-rate
|
||||
`-doppler_hz * c / 1575420000` ile hesaplanir. Ham yol tek-kanal
|
||||
leave-one-out FDE'li velocity WLS'ten ESKF'ye, filtreli yol ayri velocity
|
||||
WLS'ten yalniz PPS clock-bias drift predictor'una gider. PPS'in ana faz/zaman
|
||||
olcumu position WLS `clock_bias_m` sonucudur; Doppler yalniz 200 ms epoch'lar
|
||||
arasindaki turev prediction'ina ve kalite siniflandirmasina yardim eder. Her
|
||||
iki velocity WLS state'i `[vx, vy, vz, receiver_clock_drift_mps]`'dir; uydu
|
||||
LOS hizi ve broadcast satellite clock drift observation'dan ayrilir. C/N0,
|
||||
range-rate agirligini belirler; post-fit residual gerekirse formal
|
||||
covariance'i yalniz buyutur.
|
||||
|
||||
Klasik mutlak-pseudorange GNSS modelinde receiver zaman hatasi
|
||||
`delta_t_r(t)` ise:
|
||||
|
||||
```text
|
||||
code WLS 4. state: b_r = c * delta_t_r [metre]
|
||||
Doppler WLS 4. state: bdot_r = c * d(delta_t_r)/dt [metre/saniye]
|
||||
b_r(t + dt) ~= b_r(t) + bdot_r * dt
|
||||
```
|
||||
|
||||
Burada `bdot_r`, receiver oscillator frequency error'inin range-rate
|
||||
birimindeki ortak etkisidir. Ornegin `0.1 ppm` yaklasik `29.98 m/s` eder;
|
||||
Doppler WLS bu ortak terimi uydu yonune gore degisen gercek `vx/vy/vz`
|
||||
hareketinden ayirir.
|
||||
|
||||
Bu projede onemli bir datum farki vardir: position WLS'e verilen capture
|
||||
zamanlarindan ortak receiver zamani cikarilir ve `STARTOFFSET` eklenir. Bu
|
||||
nedenle `solution.clock_bias_m`, saf `c*delta_t_r` degil, yaklasik
|
||||
`c*STARTOFFSET - reference_range` olan referans-uyduya bagli goreli
|
||||
datumdur. Doppler WLS'nin dorduncu bilinmeyeni yine uydulara ortak receiver
|
||||
clock/frequency drift'tir; fakat iki fit arasinda
|
||||
`clock_bias_m(t+dt)=clock_bias_m(t)+clock_drift_mps*dt` kisiti kurulmaz.
|
||||
Fit'ler ayri measurement/normal-equation kullanir. Doppler WLS, LOS
|
||||
vektorleri icin position WLS'nin guncel receiver konumuna baglidir.
|
||||
|
||||
ESKF; ENU position/velocity, quaternion attitude, accel bias ve gyro bias
|
||||
iceren 15 hata durumlu filtredir. Her timestamp'li IMU ornegi prediction'a
|
||||
girer. Gecikmis GPS position ve Doppler velocity olcumleri kendi capture
|
||||
epoch'unda uygulanir, ardindan daha yeni IMU history replay edilir. Position
|
||||
ve velocity'nin ayri NIS kapilari vardir; kotu Doppler ayni epoch'taki gecerli
|
||||
position update'ini iptal etmez. Durağanlik supervisor'u uygun oldugunda ZUPT
|
||||
ve zero-gyro update uygular; GPS kesintisinde filtre sahte konum olcumu
|
||||
uretmeden dead-reckoning yapar ve covariance buyur.
|
||||
|
||||
C/N0, geometri, WLS clock sigma, integrity ve Doppler residual metrikleri PPS
|
||||
software'inin `acquire/precision/robust/holdover` profilini de secer. PPS
|
||||
donanimi bu metrikleri dogrudan yorumlamaz; software v2.2 mailbox ile PI
|
||||
`Kp/Ki`, slew ve lock esiklerini atomik olarak gunceller. Ayrintilar:
|
||||
|
||||
- `hardware/external/gps/rtl/trck_src/README.md`
|
||||
- `soc/software/gps_system/README.md`
|
||||
- `soc/software/fusion/README.md`
|
||||
- `hardware/external/gps/rtl/pps/README.md`
|
||||
- `soc/software/pps_python/README.md`
|
||||
|
||||
Alt girisler:
|
||||
|
||||
```sh
|
||||
@@ -94,6 +211,7 @@ make -C hardware/fpga/vcu118/rtl help
|
||||
make -C hardware/sim/fpga/vcu118 help
|
||||
make -C targets/fpga/vcu118 help
|
||||
make -C targets/fpga/genesys2 help
|
||||
make -C targets/fpga/ti375c529 help
|
||||
make -C targets/asic/rocket help
|
||||
```
|
||||
|
||||
@@ -104,6 +222,7 @@ FPGA karti kok Makefile'dan `FPGA_BOARD` ile secilir:
|
||||
```sh
|
||||
make FPGA_BOARD=vcu118 fpga-config
|
||||
make FPGA_BOARD=genesys2 fpga-config
|
||||
make FPGA_BOARD=ti375c529 fpga-config
|
||||
```
|
||||
|
||||
Top-level `BOARD=...` alias'i da desteklenir. Orneklerde Genesys2 kullanilir:
|
||||
@@ -117,7 +236,7 @@ Yanlislikla underscore kullanilmasi durumunda yaygin FPGA hedefleri icin
|
||||
alias'lar da vardir (`fpga_program`, `fpga_debug_openocd` gibi).
|
||||
|
||||
`fpga-config` secilen kartin clock, flash, UART, DTIM ve RF ayarlarini
|
||||
gosterir. RF kaynagi `RF_INPUT_MODE` ile secilir:
|
||||
gosterir. Genesys2 ve VCU118'de RF kaynagi `RF_INPUT_MODE` ile secilir:
|
||||
|
||||
```text
|
||||
RF_INPUT_MODE=pmod Varsayilan. Harici 4-bit RF data + RF clock PMOD'dan gelir.
|
||||
@@ -125,10 +244,14 @@ RF_INPUT_MODE=generated Dahili RF clock uretilir, RF data sifira baglanir.
|
||||
RF_INPUT_MODE=ethernet Kart bazli raw-Ethernet RF streamer path'i.
|
||||
```
|
||||
|
||||
Ti375C529 Efinity wrapper'i su anda yalniz harici PMOD RF girisini kullanir;
|
||||
bu kartta `RF_INPUT_MODE=pmod` birakilmalidir. `generated` ve `ethernet`
|
||||
yollari Ti375C529 wrapper'ina henuz uygulanmamistir.
|
||||
|
||||
Top-level FPGA wrapper'lar elle yazilmaz; `targets/fpga/*/tools/gen_*_wrapper.py`
|
||||
scriptleri tarafindan uretilir. Bu yuzden RF secimi RTL `ifdef` ile degil,
|
||||
generator parametresi ile yapilir. `make rtl` ve Vivado `build.tcl` ayni
|
||||
`RF_INPUT_MODE` degerini wrapper generator'a gecirir.
|
||||
scriptleri tarafindan uretilir. Genesys2 ve VCU118'de RF secimi RTL `ifdef`
|
||||
ile degil, generator parametresi ile yapilir. `make rtl` ve Vivado `build.tcl`
|
||||
ayni `RF_INPUT_MODE` degerini wrapper generator'a gecirir.
|
||||
|
||||
Varsayilan `pmod` modunda Ethernet RF IP/RTL okunmaz ve top'ta Ethernet PHY
|
||||
portlari olusmaz. Ethernet RF akisi gerektiginde:
|
||||
@@ -160,11 +283,23 @@ uzerinden 1.8 V bank/LVCMOS18:
|
||||
rf_data_pmod[1] AW15 PMOD0_2 J52.5 LVCMOS18 3.3 V
|
||||
rf_data_pmod[2] AV15 PMOD0_3 J52.7 LVCMOS18 3.3 V
|
||||
rf_data_pmod[3] AV16 PMOD0_4 J52.2 LVCMOS18 3.3 V
|
||||
|
||||
Ti375C529 PMOD0/J13, connector ve FPGA tarafi 3.3 V:
|
||||
port FPGA pin PMOD pin FPGA resource FPGA I/O
|
||||
rf_clk A12 J13.10 GPIOR_95_CLK12 3.3_V_LVCMOS
|
||||
rf_data[0] G16 J13.2 GPIOR_89 3.3_V_LVCMOS
|
||||
rf_data[1] F16 J13.3 GPIOR_90 3.3_V_LVCMOS
|
||||
rf_data[2] F17 J13.4 GPIOR_91 3.3_V_LVCMOS
|
||||
rf_data[3] G17 J13.7 GPIOR_92 3.3_V_LVCMOS
|
||||
```
|
||||
|
||||
VCU118'de disaridan PMOD header'a 3.3 V seviye verilir. FPGA U1 package
|
||||
pinlerine dogrudan 3.3 V uygulanmaz; o tarafta XDC `LVCMOS18` kullanir.
|
||||
|
||||
Ti375C529'de AX2771 `rf_clk` hatti mutlaka J13 pin 10'a baglanmalidir. Bu pin
|
||||
global-clock girisidir. J13 pin 1 / G15 (`GPIOR_88_PLLIN1`) PLL girisi olsa da
|
||||
dedicated fabric GCLK degildir ve bu akis RF saati icin onu kullanmaz.
|
||||
|
||||
PMOD SPI portu her RF modunda aktiftir ve ayri MMIO register block olarak
|
||||
`0x10015000` adresindedir. Kart uzerindeki konfig flash SPI'si `0x10014000`
|
||||
adresinde kalir. Yazilim driver'i single/quad calisma modunu `ctrl.bit2`
|
||||
@@ -190,6 +325,15 @@ uzerinden 1.2 V bank/LVCMOS12:
|
||||
pmod_spi_sck P30 PMOD1_3 J53.7 SCK LVCMOS12 3.3 V
|
||||
pmod_spi_dq[2] P29 PMOD1_4 J53.2 LVCMOS12 3.3 V
|
||||
pmod_spi_dq[3] L31 PMOD1_5 J53.4 LVCMOS12 3.3 V
|
||||
|
||||
Ti375C529 PMOD1/J14, connector ve FPGA tarafi 3.3 V:
|
||||
port FPGA pin PMOD pin FPGA resource FPGA I/O
|
||||
pmod_spi_csn B12 J14.1 GPIOR_96_CLK13 3.3_V_LVCMOS
|
||||
pmod_spi_dq[0] C14 J14.2 MOSI GPIOR_97 3.3_V_LVCMOS
|
||||
pmod_spi_dq[1] C13 J14.3 MISO GPIOR_98 3.3_V_LVCMOS
|
||||
pmod_spi_sck C12 J14.4 SCK GPIOR_99 3.3_V_LVCMOS
|
||||
pmod_spi_dq[2] D12 J14.7 GPIOR_100 3.3_V_LVCMOS
|
||||
pmod_spi_dq[3] F12 J14.8 GPIOR_101_PLLIN1 3.3_V_LVCMOS
|
||||
```
|
||||
|
||||
Sensorden gelen external interrupt hatti SPI'dan bagimsizdir. `pmod_irq`
|
||||
@@ -204,8 +348,16 @@ Genesys2:
|
||||
VCU118:
|
||||
port FPGA pin PMOD pin FPGA I/O PMOD header level
|
||||
pmod_irq M31 PMOD1_6 J53.6 LVCMOS12 3.3 V
|
||||
|
||||
Ti375C529:
|
||||
port FPGA pin PMOD pin FPGA resource FPGA I/O
|
||||
pmod_irq D13 J14.9 GPIOR_102 3.3_V_LVCMOS
|
||||
gps_pps E13 J14.10 GPIOR_103 3.3_V_LVCMOS
|
||||
```
|
||||
|
||||
Ti375C529'de `gps_pps` J14.10 uzerinden cikan 3.3 V PPS sinyalidir;
|
||||
`pmod_irq` ise J14.9 uzerinden FPGA'ya girer.
|
||||
|
||||
Bu pinler mod bazli companion XDC dosyalarinda tutulur:
|
||||
`hardware/fpga/genesys2/constraints/genesys2_pmod_rf.xdc`,
|
||||
`hardware/fpga/vcu118/constraints/vcu118_pmod_rf.xdc`,
|
||||
@@ -214,13 +366,45 @@ Bu pinler mod bazli companion XDC dosyalarinda tutulur:
|
||||
`hardware/fpga/genesys2/constraints/genesys2_pmod_irq.xdc`,
|
||||
`hardware/fpga/vcu118/constraints/vcu118_pmod_irq.xdc`,
|
||||
`hardware/fpga/genesys2/constraints/genesys2_ethernet.xdc` ve
|
||||
`hardware/fpga/vcu118/constraints/vcu118_ethernet.xdc`.
|
||||
`hardware/fpga/vcu118/constraints/vcu118_ethernet.xdc`. Ti375C529 pinleri,
|
||||
I/O standartlari, PLL'ler ve hard-JTAG baglantisi tek Efinity interface
|
||||
scriptinde tutulur:
|
||||
`hardware/fpga/ti375c529/constraints/ti375c529_rocket.isf`.
|
||||
|
||||
### Ti375C529 kart uzeri baglantilar
|
||||
|
||||
Ti375C529 development kit'te Rocket sistem saati kart uzerindeki 100 MHz
|
||||
osilatorden, GPS isleme saatleri ise AX2771'in J13.10 uzerindeki
|
||||
16.367981 MHz RF saatinden uretilir. SW3 active-low Rocket resetidir. FTDI
|
||||
UART varsayilan olarak 460800 baud ve mevcut Linux enumeration'inda
|
||||
`/dev/ttyUSB2` kullanir. Birden fazla kart veya farkli enumeration icin
|
||||
`UART_PORT=...` ve `FTDI_URL=...` ile override edilebilir.
|
||||
|
||||
Lower 512-Mbit konfig flash'i kart icinde su pinlere baglidir; PMOD'a kablo
|
||||
cekilmez:
|
||||
|
||||
```text
|
||||
port FPGA pin FPGA resource
|
||||
flash_csn A7 GPIOB_N_38 / SSL_N
|
||||
flash_sck G6 GPIOB_P_39 / CCK
|
||||
flash_dq[0] H4 GPIOB_N_40 / CDI0
|
||||
flash_dq[1] H5 GPIOB_P_40 / CDI1
|
||||
flash_dq[2] G5 GPIOB_N_39 / CDI2
|
||||
flash_dq[3] B7 GPIOB_P_38 / CDI3
|
||||
```
|
||||
|
||||
Efinity `JTAG_USER1`, mevcut Rocket DMI tunnel protokolunu kullanir. Host
|
||||
OpenOCD build'inde `riscv set_bscan_tunnel_ir` destegi bulunmalidir; Efinity
|
||||
ile gelen OpenOCD yerine uyumlu host OpenOCD kullanilir. Ayrintili Efinity,
|
||||
clock, JTAG ve dogrulama notlari icin
|
||||
`targets/fpga/ti375c529/README.md` dosyasina bakin.
|
||||
|
||||
## FPGA Flash ve MCS Akislari
|
||||
|
||||
Bu hedefler kok Makefile'dan secilen karta yonlenir. Genesys2 icin
|
||||
`BOARD=genesys2`, VCU118 icin `BOARD=vcu118` kullanilir. `APP` verilmezse
|
||||
varsayilan uygulama `hello_uart` olur.
|
||||
`BOARD=genesys2`, VCU118 icin `BOARD=vcu118`, Ti375C529 icin
|
||||
`BOARD=ti375c529` kullanilir. `APP` verilmezse varsayilan uygulama
|
||||
`hello_uart` olur.
|
||||
|
||||
Sadece ELF'i derleyip bootloader'in okuyacagi flash-format app imajina cevirmek:
|
||||
|
||||
@@ -317,6 +501,57 @@ flash size: 32 MiB
|
||||
interface: SPIx4
|
||||
```
|
||||
|
||||
### Ti375C529 Efinity build ve flash
|
||||
|
||||
Efinity varsayilan olarak `/tools/efinix/efinity` altinda aranir. Farkli bir
|
||||
kurulum `EFINITY_HOME=/path/to/efinity` ile secilebilir. Build, SRAM'e gecici
|
||||
programlama ve kart tespiti kokten su sekilde yapilir:
|
||||
|
||||
```sh
|
||||
make FPGA_BOARD=ti375c529 fpga-config
|
||||
make FPGA_BOARD=ti375c529 fpga-bitstream
|
||||
make -C targets/fpga/ti375c529 detect
|
||||
make FPGA_BOARD=ti375c529 fpga-program
|
||||
```
|
||||
|
||||
Ti375C529'de repository genelindeki `fpga-mcs` hedef adi korunur fakat cikti
|
||||
Xilinx MCS/BIN degil, Efinity raw HEX'tir. Lower 512-Mbit flash yerlesimi:
|
||||
|
||||
```text
|
||||
configuration offset: 0x00000000
|
||||
app offset: 0x00800000
|
||||
flash size: 64 MiB
|
||||
interface: active-x1 configuration / user-mode quad SPI
|
||||
```
|
||||
|
||||
Konfig ve uygulama imajlarini uretmek ve programlamak icin:
|
||||
|
||||
```sh
|
||||
make FPGA_BOARD=ti375c529 APP=hello_uart fpga-mcs
|
||||
make FPGA_BOARD=ti375c529 APP=hello_uart fpga-flash-app
|
||||
make FPGA_BOARD=ti375c529 fpga-flash-bitstream
|
||||
make FPGA_BOARD=ti375c529 APP=hello_uart fpga-flash-mcs
|
||||
```
|
||||
|
||||
Ciktilar:
|
||||
|
||||
```text
|
||||
targets/fpga/ti375c529/out/bitstream/ti375c529_rocket.bit
|
||||
targets/fpga/ti375c529/out/bitstream/ti375c529_rocket.hex
|
||||
targets/fpga/ti375c529/out/flash/ti375c529_rocket.hex
|
||||
targets/fpga/ti375c529/out/flash/ti375c529_rocket.json
|
||||
```
|
||||
|
||||
Flash programlama Efinity'nin Ti375'e uygun JTAG-to-SPI bridge'ini gecici
|
||||
olarak yukler. Gercek persistent boot testi icin programlamadan sonra karta
|
||||
power-cycle uygulanmalidir. GDB ile DTIM'e yukleme ve UART dinleme:
|
||||
|
||||
```sh
|
||||
make FPGA_BOARD=ti375c529 fpga-debug-openocd
|
||||
make FPGA_BOARD=ti375c529 APP=hello_uart fpga-debug-load
|
||||
make FPGA_BOARD=ti375c529 fpga-uart
|
||||
```
|
||||
|
||||
## GPS Reset Davranisi
|
||||
|
||||
OpenOCD/GDB `monitor reset` yolu Rocket debug module `ndmreset` sinyalini
|
||||
|
||||
@@ -46,3 +46,9 @@ external/gps/rtl/
|
||||
pass this path as the default GPS_SYSTEM_DIR, so the Vivado flow does not
|
||||
depend on a user-specific /home/... checkout.
|
||||
```
|
||||
|
||||
GPS tracking olcum yolunun 20 ms Doppler/C/N0 snapshot, PCLK synchronizer,
|
||||
APB register/interrupt ve software velocity WLS sozlesmesi
|
||||
`external/gps/rtl/trck_src/README.md` icinde belgelenir. PPS PI periferigi,
|
||||
runtime coefficient/lock register map'i ve fiziksel pin sinirlari
|
||||
`external/gps/rtl/pps/README.md` icindedir.
|
||||
|
||||
+32
@@ -47,6 +47,38 @@ The hardware does not add `POSTPONE_TOW`. Baremetal must commit an already
|
||||
epoch-aligned GPS reference, so the verified 160 ms subframe readout correction
|
||||
is owned exactly once by the navigation software.
|
||||
|
||||
Tracking C/N0 and Doppler are not inputs to the PPS RTL. The tracking wrapper
|
||||
publishes those measurements through its separate 20 ms APB mailbox. Firmware
|
||||
uses C/N0, geometry, WLS clock sigma, integrity and Doppler velocity residual
|
||||
to select `acquire`, `precision`, `robust` or `holdover`, then writes the
|
||||
corresponding PI coefficients, slew limit and lock thresholds through this
|
||||
PPS mailbox. Doppler also predicts the WLS clock-bias datum between commits.
|
||||
This keeps quality policy and the small navigation pre-filter in software
|
||||
while the deterministic PI loop, fixed-point rate state and physical edge
|
||||
generation remain in hardware.
|
||||
|
||||
In a conventional absolute-pseudorange model, receiver clock bias and
|
||||
receiver clock drift are the level and slope of the same physical clock:
|
||||
`b_r = c*delta_t_r` and `bdot_r = c*d(delta_t_r)/dt`. The position-WLS bias
|
||||
in this project is different: subtracting the common capture time and adding
|
||||
`STARTOFFSET` makes it a reference-satellite-relative datum, approximately
|
||||
`c*STARTOFFSET - reference_range`, rather than a pure receiver oscillator
|
||||
offset.
|
||||
|
||||
The Doppler WLS still solves a fourth, common receiver clock/frequency-drift
|
||||
state. The PPS pre-filter does not use that state directly as the derivative
|
||||
of the position-WLS datum. Instead, it predicts that datum with:
|
||||
|
||||
```text
|
||||
position_datum_drift =
|
||||
-reference_geometric_range_rate
|
||||
+ c * reference_satellite_clock_drift
|
||||
```
|
||||
|
||||
The geometric term uses the Doppler-WLS receiver velocity plus the selected
|
||||
reference satellite position and velocity. The PPS RTL sees only the
|
||||
resulting timestamped reference and loop configuration.
|
||||
|
||||
The APB register offsets live in `pps_pkg.sv`. Multiword input fields are
|
||||
shadow registers and must be followed by one `COMMIT` write. A new mailbox
|
||||
must not be written while `STATUS.COMMIT_BUSY` is set.
|
||||
|
||||
+134
@@ -0,0 +1,134 @@
|
||||
# GPS tracking measurement yolu
|
||||
|
||||
Bu dizindeki tracking loop yalniz code/carrier takibi yapmaz; her 20 ms GPS
|
||||
L1 C/A navigation-bit epoch'unda software'in C/N0 ve Doppler kullanabilmesi
|
||||
icin atomik bir measurement snapshot'i da uretir.
|
||||
|
||||
## 20 ms snapshot
|
||||
|
||||
`tracking_measurement.sv`, ilk NAV-bit kenarinda accumulator'lari arm eder ve
|
||||
sonraki her bit epoch'unda su alanlari birlikte latch eder:
|
||||
|
||||
```text
|
||||
nav bit
|
||||
ABS_SAMPLE
|
||||
remaining code phase
|
||||
signed carrier frequency, Q7
|
||||
20 x prompt I toplami
|
||||
20 x prompt Q toplami
|
||||
20 x (I_prompt^2 + Q_prompt^2) wideband power toplami
|
||||
sample count
|
||||
16-bit sequence
|
||||
```
|
||||
|
||||
Prompt energy `lock_detector.sv` icinde pipeline'li oldugu icin bit kenarindan
|
||||
sonra dort tracking clock beklenir; bu, son 1 ms enerji terimini eski epoch'a
|
||||
dahil eder ve yeni epoch ile karistirmaz. Snapshot ve `measurement_valid`
|
||||
sonraki bit epoch'una kadar, yaklasik 20 ms sabit kalir.
|
||||
|
||||
## APB aktarimi ve interrupt
|
||||
|
||||
`apb_wrappers/tracking_wrapper.sv`, valid ve payload'un her bitini
|
||||
`double_ff_syn` yardimcisi ile PCLK domainine alir. Bu yardimcinin mevcut
|
||||
implementasyonu `stage_1`, `stage_2` ve registered `Dout` olmak uzere uc
|
||||
register kademesidir. Kaynak payload 20 ms sabit kaldigi icin FIFO veya toggle
|
||||
handshake kullanilmaz. Wrapper synchronized valid kenarindan sonra iki ek PCLK
|
||||
settle cycle bekler, tum alanlari APB-domain register bankina ayni anda kopyalar
|
||||
ve measurement interrupt/flag uretir.
|
||||
|
||||
Software sequence'i okumadan once ve payload'dan sonra tekrar okuyarak atomik
|
||||
snapshot dogrulamasi yapar. Sequence degisirse okuma tekrar edilir; sequence
|
||||
atlarsa kayip measurement diagnostics sayaci artar.
|
||||
|
||||
Measurement register offsetleri:
|
||||
|
||||
| Offset | Alan |
|
||||
|---:|---|
|
||||
| `0x50` | version, valid, NAV bit, lock ve sample count metadata |
|
||||
| `0x54` | 16-bit sequence |
|
||||
| `0x58` | signed carrier frequency Q7 |
|
||||
| `0x5c` | ABS_SAMPLE |
|
||||
| `0x60` | remaining code phase |
|
||||
| `0x64` | signed prompt sum I |
|
||||
| `0x68` | signed prompt sum Q |
|
||||
| `0x6c` | wideband power low |
|
||||
| `0x70` | wideband power high |
|
||||
|
||||
## Software'e verilen anlam
|
||||
|
||||
Carrier register'i toplam NCO frekansidir. Software IF merkezini cikarir:
|
||||
|
||||
```text
|
||||
doppler_hz = carrier_q7 / 128 - 4092000
|
||||
range_rate_mps = -doppler_hz * c / 1575420000
|
||||
```
|
||||
|
||||
Pozitif received-frequency Doppler azalan geometrik range anlamina geldigi
|
||||
icin range-rate denkleminde eksi isareti vardir.
|
||||
|
||||
C/N0, 20 prompt icin narrowband/wideband power estimator'i ile hesaplanir:
|
||||
|
||||
```text
|
||||
R = (sum_i^2 + sum_q^2) / WBP
|
||||
C/N0_linear = 1000 * (R - 1) / (20 - R)
|
||||
C/N0_dbhz = 10 * log10(C/N0_linear)
|
||||
```
|
||||
|
||||
RTL IIR uygulamaz. Ham snapshot debug ve yeniden tune icin korunur. ISR state'i
|
||||
ayni PRN'de C/N0 icin `alpha=0.2`, Doppler icin `alpha=0.05` software IIR
|
||||
uygular; reacquisition veya PRN degisiminde eski kanal state'i kullanilmaz.
|
||||
|
||||
Filtrelenmis range-rate, satellite ECEF velocity ve satellite clock drift ile
|
||||
C/N0-weighted 4-state velocity WLS'ye girer:
|
||||
|
||||
```text
|
||||
state = [receiver_vx, receiver_vy, receiver_vz, receiver_clock_drift_mps]
|
||||
observation = measured_range_rate
|
||||
- satellite_LOS_rate
|
||||
+ c * satellite_clock_drift
|
||||
H = [-u_x, -u_y, -u_z, 1]
|
||||
```
|
||||
|
||||
Buradaki dorduncu state, tum uydu Doppler observation'larina ortak giren
|
||||
receiver clock/frequency drift'idir. Klasik mutlak-pseudorange modelinde
|
||||
receiver zaman hatasi `delta_t_r(t)` icin:
|
||||
|
||||
```text
|
||||
clock_bias_m = c * delta_t_r(t)
|
||||
clock_drift_mps = c * d(delta_t_r(t))/dt
|
||||
clock_bias_m(t+dt) ~= clock_bias_m(t) + clock_drift_mps * dt
|
||||
```
|
||||
|
||||
`clock_drift_mps`, receiver local oscillator frequency error'inin range-rate
|
||||
birimindeki ortak etkisidir. `0.1 ppm`, yaklasik `29.98 m/s`'ye karsilik
|
||||
gelir. Bu ortak state olmadan oscillator hatasi receiver hareketinin bir
|
||||
parcasi sanilirdi.
|
||||
|
||||
Bu projedeki position WLS bias'i ise mutlak receiver clock bias'i degildir.
|
||||
Ortak capture zamani cikarildigi ve `STARTOFFSET` eklendigi icin
|
||||
referans-uyduya bagli goreli bir datum cozer. Bu nedenle Doppler WLS
|
||||
`clock_drift_mps` state'i, position WLS `clock_bias_m` degerinin turevi olarak
|
||||
dogrudan baglanmaz.
|
||||
|
||||
Iki WLS ayri observation ve normal equation kullanir; fakat Doppler WLS tam
|
||||
bagimsiz degildir. Uydu LOS vektorlerini kurmak icin position WLS'den gelen
|
||||
guncel receiver ECEF konumunu, ephemeristen gelen uydu ECEF konum/hiz ve clock
|
||||
drift'ini kullanir. Satellite clock drift bilinen duzeltmedir; WLS'nin
|
||||
dorduncu bilinmeyeni receiver clock drift'idir.
|
||||
|
||||
WLS en az dort gecerli uydu ister; ECEF velocity, 3x3 covariance, receiver
|
||||
clock drift ve residual RMS uretir. Fusion adapter velocity/covariance'i ENU'ya
|
||||
dondurur ve ayri velocity NIS kapisi ile 15-state ESKF'ye verir.
|
||||
|
||||
## Dogrulama
|
||||
|
||||
```sh
|
||||
make -C hardware/external/gps/rtl/trck_src/tb test
|
||||
make -C soc/software/gps_system/tests run
|
||||
make -C soc/software/gps_system/tests sanitize
|
||||
```
|
||||
|
||||
Questa testleri 20 prompt akumulasyonu, snapshot hold, synchronizer settle,
|
||||
atomik APB copy ve interrupt'i; Linux C testleri Q7 Doppler/range-rate, C/N0
|
||||
estimator, coherent read, satellite velocity/clock drift ve weighted velocity
|
||||
WLS/covariance yolunu kontrol eder.
|
||||
@@ -0,0 +1,226 @@
|
||||
# Efinity Interface Configuration for the Ti375C529 development kit.
|
||||
# Pin assignments follow the kit user guide and the Efinity 2025.2 device DB.
|
||||
|
||||
# Board I/O bank voltages. Keep these aligned with the Efinix out-of-box
|
||||
# Ti375C529 development-kit project.
|
||||
design.set_device_property("2A", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("2A", "2A_MODE_SEL", "2A")
|
||||
design.set_device_property("2A", "VOLTAGE", "1.8", "IOBANK")
|
||||
design.set_device_property("2B", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("2B", "2B_MODE_SEL", "2B")
|
||||
design.set_device_property("2B", "VOLTAGE", "1.8", "IOBANK")
|
||||
design.set_device_property("2C", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("2C", "2C_MODE_SEL", "2C")
|
||||
design.set_device_property("2C", "VOLTAGE", "1.8", "IOBANK")
|
||||
design.set_device_property("2D", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("2D", "2D_MODE_SEL", "2D")
|
||||
design.set_device_property("2D", "VOLTAGE", "1.8", "IOBANK")
|
||||
design.set_device_property("2E", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("2E", "2E_MODE_SEL", "2E")
|
||||
design.set_device_property("2E", "VOLTAGE", "1.8", "IOBANK")
|
||||
design.set_device_property("4A_4B", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("4A_4B", "4A_4B_MODE_SEL", "4A_4B")
|
||||
design.set_device_property("4A_4B", "VOLTAGE", "1.8", "IOBANK")
|
||||
design.set_device_property("4C", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("4C", "4C_MODE_SEL", "4C")
|
||||
design.set_device_property("4C", "VOLTAGE", "1.8", "IOBANK")
|
||||
design.set_device_property("4D", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("4D", "4D_MODE_SEL", "4D")
|
||||
design.set_device_property("4D", "VOLTAGE", "1.8", "IOBANK")
|
||||
design.set_device_property("BL2_BL3", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("BL2_BL3", "BL2_MODE_SEL", "BL2")
|
||||
design.set_mode_sel_name("BL2_BL3", "BL3_MODE_SEL", "BL3")
|
||||
design.set_device_property("BL2_BL3", "VOLTAGE", "3.3", "IOBANK")
|
||||
design.set_device_property("BR0", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("BR0", "BR0_MODE_SEL", "BR0")
|
||||
design.set_device_property("BR0", "VOLTAGE", "3.3", "IOBANK")
|
||||
design.set_device_property("BR3_BR4", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("BR3_BR4", "BR3_MODE_SEL", "BR3")
|
||||
design.set_mode_sel_name("BR3_BR4", "BR4_MODE_SEL", "BR4")
|
||||
design.set_device_property("BR3_BR4", "VOLTAGE", "3.3", "IOBANK")
|
||||
design.set_device_property("TL1_TL5", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("TL1_TL5", "TL1_MODE_SEL", "TL1")
|
||||
design.set_mode_sel_name("TL1_TL5", "TL5_MODE_SEL", "TL5")
|
||||
design.set_device_property("TL1_TL5", "VOLTAGE", "3.3", "IOBANK")
|
||||
design.set_device_property("TR0", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("TR0", "TR0_MODE_SEL", "TR0")
|
||||
design.set_device_property("TR0", "VOLTAGE", "3.3", "IOBANK")
|
||||
design.set_device_property("TR1", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("TR1", "TR1_MODE_SEL", "TR1")
|
||||
design.set_device_property("TR1", "VOLTAGE", "3.3", "IOBANK")
|
||||
design.set_device_property("TR2", "DYNAMIC_VOLTAGE", "0", "IOBANK")
|
||||
design.set_mode_sel_name("TR2", "TR2_MODE_SEL", "TR2")
|
||||
design.set_device_property("TR2", "VOLTAGE", "3.3", "IOBANK")
|
||||
|
||||
# External GPIO.
|
||||
design.create_input_gpio("reset_n")
|
||||
design.set_property("reset_n", "PULL_OPTION", "WEAK_PULLUP")
|
||||
design.create_input_gpio("uart_rx")
|
||||
design.create_output_gpio("uart_tx")
|
||||
# Lower 512-Mbit configuration flash. After active configuration completes,
|
||||
# these dedicated SPI pins become fabric GPIO and are driven by Rocket's MMIO
|
||||
# SPI controller so the BootROM can read the application region.
|
||||
design.create_output_gpio("flash_sck")
|
||||
design.create_output_gpio("flash_csn")
|
||||
design.create_inout_gpio("flash_dq", 3, 0)
|
||||
# The AX2771 sample clock is routed to J13 pin 10 / GPIOR_95_CLK12 so it
|
||||
# enters the fabric through a dedicated global-clock input instead of being
|
||||
# promoted from normal GPIO routing by the placer.
|
||||
design.create_input_clock_gpio("rf_clk")
|
||||
design.create_input_gpio("rf_data", 3, 0)
|
||||
design.create_output_gpio("pmod_spi_sck")
|
||||
design.create_output_gpio("pmod_spi_csn")
|
||||
design.create_inout_gpio("pmod_spi_dq", 3, 0)
|
||||
design.create_input_gpio("pmod_irq")
|
||||
design.create_output_gpio("gps_pps")
|
||||
design.create_output_gpio("led", 4, 0)
|
||||
|
||||
# The two PMOD headers, FTDI UART, reset switch, 100 MHz oscillator, and
|
||||
# LED9/LED10 use the development kit's 3.3 V banks. GPIOs otherwise default
|
||||
# to 1.8 V LVCMOS, so state these assignments explicitly.
|
||||
design.set_property("reset_n", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("uart_rx", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("uart_tx", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("flash_sck", "IO_STANDARD", "1.8_V_LVCMOS")
|
||||
design.set_property("flash_csn", "IO_STANDARD", "1.8_V_LVCMOS")
|
||||
design.set_property("flash_dq[0]", "IO_STANDARD", "1.8_V_LVCMOS")
|
||||
design.set_property("flash_dq[1]", "IO_STANDARD", "1.8_V_LVCMOS")
|
||||
design.set_property("flash_dq[2]", "IO_STANDARD", "1.8_V_LVCMOS")
|
||||
design.set_property("flash_dq[3]", "IO_STANDARD", "1.8_V_LVCMOS")
|
||||
design.set_property("flash_csn", "PULL_OPTION", "WEAK_PULLUP")
|
||||
design.set_property("flash_dq[0]", "PULL_OPTION", "WEAK_PULLUP")
|
||||
design.set_property("flash_dq[1]", "PULL_OPTION", "WEAK_PULLUP")
|
||||
design.set_property("flash_dq[2]", "PULL_OPTION", "WEAK_PULLUP")
|
||||
design.set_property("flash_dq[3]", "PULL_OPTION", "WEAK_PULLUP")
|
||||
design.set_property("rf_clk", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("rf_data[0]", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("rf_data[1]", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("rf_data[2]", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("rf_data[3]", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("pmod_spi_sck", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("pmod_spi_csn", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("pmod_spi_dq[0]", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("pmod_spi_dq[1]", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("pmod_spi_dq[2]", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("pmod_spi_dq[3]", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("pmod_irq", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("gps_pps", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("led[0]", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("led[1]", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
|
||||
# Rocket/fabric clock: 100 MHz board oscillator -> 100 MHz GCLK.
|
||||
design.create_block("fabric_pll", "PLL")
|
||||
design.gen_pll_ref_clock("fabric_pll", pll_res="PLL_BL0", refclk_src="EXTERNAL", refclk_name="board_clk_100", ext_refclk_no="1")
|
||||
design.set_property("board_clk_100", "IO_STANDARD", "3.3_V_LVCMOS")
|
||||
design.set_property("fabric_pll", "REFCLK_FREQ", "100", "PLL")
|
||||
design.set_property("fabric_pll", "CLKOUT0_PIN", "fabric_clk", "PLL")
|
||||
design.set_property("fabric_pll", "CLKOUT0_CONN_TYPE", "gclk", "PLL")
|
||||
design.set_property("fabric_pll", "LOCKED_PIN", "fabric_pll_locked", "PLL")
|
||||
design.auto_calc_pll_clock("fabric_pll", {"CLKOUT0_FREQ": "100.0", "CLKOUT0_PHASE": "0"})
|
||||
|
||||
# RF-coherent GPS processing clocks. The AX2771's 16.367981 MHz clock enters
|
||||
# the fabric on J13 pin 10 / A12 (GPIOR_95_CLK12) and drives the same two-stage
|
||||
# clock chain used by the Xilinx targets:
|
||||
# RF -> ~55.55556 MHz -> 100 MHz acquisition / 125 MHz tracking.
|
||||
design.create_block("gps_ref_pll", "PLL")
|
||||
design.gen_pll_ref_clock("gps_ref_pll", pll_res="PLL_TR0", refclk_src="CORE")
|
||||
design.set_property("gps_ref_pll", "CORE_CLK_PIN", "rf_clk", "PLL")
|
||||
design.set_property("gps_ref_pll", "REFCLK_FREQ", "16.367981", "PLL")
|
||||
design.set_property("gps_ref_pll", "CLKOUT1_EN", "1", "PLL")
|
||||
design.set_property("gps_ref_pll", "CLKOUT0_PIN", "gps_ref_clk_gen", "PLL")
|
||||
design.set_property("gps_ref_pll", "CLKOUT0_CONN_TYPE", "gclk", "PLL")
|
||||
design.set_property("gps_ref_pll", "CLKOUT1_PIN", "gps_ref_feedback", "PLL")
|
||||
design.set_property("gps_ref_pll", "CLKOUT1_CONN_TYPE", "gclk", "PLL")
|
||||
design.set_property("gps_ref_pll", "LOCKED_PIN", "gps_ref_pll_locked", "PLL")
|
||||
design.set_property("gps_ref_pll", "FEEDBACK_MODE", "LOCAL", "PLL")
|
||||
design.set_property("gps_ref_pll", "FEEDBACK_CLK", "CLK1", "PLL")
|
||||
design.set_property("gps_ref_pll", "FRACTIONAL_MODE_EN", "1", "PLL")
|
||||
design.set_property("gps_ref_pll", "FRACTIONAL_COEFFICIENT", "10715375", "PLL")
|
||||
design.set_property("gps_ref_pll", "M", "4", "PLL")
|
||||
design.set_property("gps_ref_pll", "N", "1", "PLL")
|
||||
design.set_property("gps_ref_pll", "O", "1", "PLL")
|
||||
design.set_property("gps_ref_pll", "CLKOUT0_DIV", "42", "PLL")
|
||||
design.set_property("gps_ref_pll", "CLKOUT1_DIV", "35", "PLL")
|
||||
design.set_property("gps_ref_pll", "CLKOUT1_PDIV", "5", "PLL")
|
||||
design.set_property("gps_ref_pll", "CLKOUT1_SDIV", "30", "PLL")
|
||||
design.calc_pll_clock("gps_ref_pll")
|
||||
|
||||
design.create_block("gps_domain_pll", "PLL")
|
||||
design.gen_pll_ref_clock("gps_domain_pll", pll_res="PLL_TR1", refclk_src="CORE")
|
||||
design.set_property("gps_domain_pll", "CORE_CLK_PIN", "gps_ref_clk_gen", "PLL")
|
||||
design.set_property("gps_domain_pll", "REFCLK_FREQ", "55.55555593", "PLL")
|
||||
design.set_property("gps_domain_pll", "CLKOUT3_EN", "1", "PLL")
|
||||
design.set_property("gps_domain_pll", "CLKOUT4_EN", "1", "PLL")
|
||||
design.set_property("gps_domain_pll", "CLKOUT0_PIN", "gps_domain_feedback", "PLL")
|
||||
design.set_property("gps_domain_pll", "CLKOUT0_CONN_TYPE", "gclk", "PLL")
|
||||
design.set_property("gps_domain_pll", "CLKOUT3_PIN", "gps_acq_clk_gen", "PLL")
|
||||
design.set_property("gps_domain_pll", "CLKOUT3_CONN_TYPE", "gclk", "PLL")
|
||||
design.set_property("gps_domain_pll", "CLKOUT4_PIN", "gps_trck_clk_gen", "PLL")
|
||||
design.set_property("gps_domain_pll", "CLKOUT4_CONN_TYPE", "gclk", "PLL")
|
||||
design.set_property("gps_domain_pll", "LOCKED_PIN", "gps_domain_pll_locked", "PLL")
|
||||
design.set_property("gps_domain_pll", "FEEDBACK_MODE", "LOCAL", "PLL")
|
||||
design.set_property("gps_domain_pll", "FEEDBACK_CLK", "CLK0", "PLL")
|
||||
design.set_property("gps_domain_pll", "M", "2", "PLL")
|
||||
design.set_property("gps_domain_pll", "N", "1", "PLL")
|
||||
design.set_property("gps_domain_pll", "O", "2", "PLL")
|
||||
design.set_property("gps_domain_pll", "CLKOUT0_DIV", "18", "PLL")
|
||||
design.set_property("gps_domain_pll", "CLKOUT3_DIV", "20", "PLL")
|
||||
design.set_property("gps_domain_pll", "CLKOUT4_DIV", "16", "PLL")
|
||||
design.calc_pll_clock("gps_domain_pll")
|
||||
|
||||
# Hard JTAG USER1 exposes the TAP handshake at the FPGA core boundary.
|
||||
design.create_block("rocket_jtag", "JTAG")
|
||||
design.set_property("rocket_jtag", "NAME", "rocket_jtag", "JTAG")
|
||||
design.set_property("rocket_jtag", "RESOURCE", "JTAG_USER1", "JTAG")
|
||||
design.set_property("rocket_jtag", "CAPTURE", "jtag_CAPTURE", "JTAG")
|
||||
design.set_property("rocket_jtag", "DRCK", "jtag_DRCK", "JTAG")
|
||||
design.set_property("rocket_jtag", "RESET", "jtag_RESET", "JTAG")
|
||||
design.set_property("rocket_jtag", "RUNTEST", "jtag_RUNTEST", "JTAG")
|
||||
design.set_property("rocket_jtag", "SEL", "jtag_SEL", "JTAG")
|
||||
design.set_property("rocket_jtag", "SHIFT", "jtag_SHIFT", "JTAG")
|
||||
design.set_property("rocket_jtag", "TCK", "jtag_TCK", "JTAG")
|
||||
design.set_property("rocket_jtag", "TDI", "jtag_TDI", "JTAG")
|
||||
design.set_property("rocket_jtag", "TMS", "jtag_TMS", "JTAG")
|
||||
design.set_property("rocket_jtag", "UPDATE", "jtag_UPDATE", "JTAG")
|
||||
design.set_property("rocket_jtag", "TDO", "jtag_TDO", "JTAG")
|
||||
design.assign_resource("rocket_jtag", "JTAG_USER1", "JTAG")
|
||||
|
||||
# Package assignments.
|
||||
design.assign_pkg_pin("board_clk_100", "U4")
|
||||
design.assign_pkg_pin("reset_n", "U19")
|
||||
design.assign_pkg_pin("uart_rx", "E10")
|
||||
design.assign_pkg_pin("uart_tx", "E9")
|
||||
|
||||
# Lower configuration flash U47: CDI0..3, CCK, and SSL_N. These assignments
|
||||
# match Efinix's Ti375C529 JTAG-to-SPI loader reference design.
|
||||
design.assign_pkg_pin("flash_dq[0]", "H4")
|
||||
design.assign_pkg_pin("flash_dq[1]", "H5")
|
||||
design.assign_pkg_pin("flash_dq[2]", "G5")
|
||||
design.assign_pkg_pin("flash_dq[3]", "B7")
|
||||
design.assign_pkg_pin("flash_sck", "G6")
|
||||
design.assign_pkg_pin("flash_csn", "A7")
|
||||
|
||||
# PMOD0/J13 RF input. J13 pin 10 is the header's dedicated GCLK-capable pin;
|
||||
# J13 pin 1 / G15 is intentionally unused.
|
||||
design.assign_pkg_pin("rf_clk", "A12")
|
||||
design.assign_pkg_pin("rf_data[0]", "G16")
|
||||
design.assign_pkg_pin("rf_data[1]", "F16")
|
||||
design.assign_pkg_pin("rf_data[2]", "F17")
|
||||
design.assign_pkg_pin("rf_data[3]", "G17")
|
||||
|
||||
# PMOD1/J14 SPI + interrupt + PPS.
|
||||
design.assign_pkg_pin("pmod_spi_csn", "B12")
|
||||
design.assign_pkg_pin("pmod_spi_dq[0]", "C14")
|
||||
design.assign_pkg_pin("pmod_spi_dq[1]", "C13")
|
||||
design.assign_pkg_pin("pmod_spi_sck", "C12")
|
||||
design.assign_pkg_pin("pmod_spi_dq[2]", "D12")
|
||||
design.assign_pkg_pin("pmod_spi_dq[3]", "F12")
|
||||
design.assign_pkg_pin("pmod_irq", "D13")
|
||||
design.assign_pkg_pin("gps_pps", "E13")
|
||||
|
||||
# User LEDs LED9..LED13.
|
||||
design.assign_pkg_pin("led[0]", "V4")
|
||||
design.assign_pkg_pin("led[1]", "U6")
|
||||
design.assign_pkg_pin("led[2]", "L18")
|
||||
design.assign_pkg_pin("led[3]", "R20")
|
||||
design.assign_pkg_pin("led[4]", "E19")
|
||||
@@ -0,0 +1,8 @@
|
||||
# AX2771 sample clock on PMOD J13. The 100/125 MHz GPS clocks generated by the
|
||||
# two Efinity PLLs are frequency-related to this input; the Rocket fabric clock
|
||||
# comes from the independent on-board 100 MHz oscillator.
|
||||
create_clock -period 61.09495 -name rf_clk [get_ports {rf_clk}]
|
||||
|
||||
# The FTDI adapter is normally run at 1 MHz. Constraining the hard-TAP fabric
|
||||
# interface to 10 MHz leaves margin if the debug adapter speed is increased.
|
||||
create_clock -period 100.000 -name jtag_TCK [get_ports {jtag_TCK}]
|
||||
@@ -0,0 +1,10 @@
|
||||
# Efinity's generated .pt.sdc is read before this file and defines the fabric
|
||||
# and PLL-output clocks. The Rocket oscillator, AX2771-derived GPS clocks, and
|
||||
# hard-TAP JTAG clock are independent domains.
|
||||
set_clock_groups -asynchronous \
|
||||
-group [get_clocks {fabric_clk}] \
|
||||
-group [get_clocks {rf_clk}] \
|
||||
-group [get_clocks {gps_ref_clk_gen}] \
|
||||
-group [get_clocks {gps_acq_clk_gen}] \
|
||||
-group [get_clocks {gps_trck_clk_gen}] \
|
||||
-group [get_clocks {jtag_TCK}]
|
||||
@@ -0,0 +1,18 @@
|
||||
.PHONY: help all shell generated clean
|
||||
|
||||
help:
|
||||
@echo "Ti375C529 RTL targets:"
|
||||
@echo " make shell # list hand-written FPGA shell RTL"
|
||||
@echo " make generated # generate Rocket SystemVerilog and Efinix wrapper"
|
||||
@echo " make clean # remove generated wrapper/verilog"
|
||||
|
||||
all: generated
|
||||
|
||||
shell:
|
||||
@find . -type f -name '*.sv' -print | sort
|
||||
|
||||
generated:
|
||||
@$(MAKE) -C ../../../../targets/fpga/ti375c529 rtl
|
||||
|
||||
clean:
|
||||
rm -rf ../../../generated/ti375c529-soc-verilog ../../../../targets/fpga/ti375c529/out/build/generated
|
||||
+4
-3
@@ -18,9 +18,9 @@ JDK_SHA256="d8afc263758141a66e0e3aafc321e783f7016696f4eaea067d340a269037d331"
|
||||
MILL_URL="https://github.com/com-lihaoyi/mill/releases/download/$MILL_VERSION/$MILL_VERSION"
|
||||
MILL_SHA256="ebe83649cd63acd33608f6d14452c074be2ed7d00cd2f69f88573d3f6909774f"
|
||||
|
||||
CIRCT_ARCHIVE="circt-full-shared-linux-x64.tar.gz"
|
||||
CIRCT_ARCHIVE="firrtl-bin-linux-x64.tar.gz"
|
||||
CIRCT_URL="https://github.com/llvm/circt/releases/download/firtool-$FIRTOOL_VERSION/$CIRCT_ARCHIVE"
|
||||
CIRCT_SHA256="dde442dcd0ada92e6f7db26f4744667471f7be61d10f2876297efb435fc9a5ec"
|
||||
CIRCT_SHA256="10bf9b135a65e5d93fb0555e967eac4258400ee629731ba68a69be6b7f9781ce"
|
||||
|
||||
Z3_ARCHIVE="z3-$Z3_VERSION-x64-glibc-2.39.zip"
|
||||
Z3_URL="https://github.com/Z3Prover/z3/releases/download/z3-$Z3_VERSION/$Z3_ARCHIVE"
|
||||
@@ -85,7 +85,8 @@ if [[ ! -x "$TOOLS_ROOT/mill/$MILL_VERSION/mill" ]]; then
|
||||
"${SUDO[@]}" install -m 0755 "$work_dir/mill" "$TOOLS_ROOT/mill/$MILL_VERSION/mill"
|
||||
fi
|
||||
|
||||
if [[ ! -x "$TOOLS_ROOT/circt/firtool-$FIRTOOL_VERSION/bin/firtool" ]]; then
|
||||
if ! "$TOOLS_ROOT/circt/firtool-$FIRTOOL_VERSION/bin/firtool" --version \
|
||||
>/dev/null 2>&1; then
|
||||
download "$CIRCT_URL" "$work_dir/$CIRCT_ARCHIVE" "$CIRCT_SHA256"
|
||||
mkdir -p "$work_dir/circt"
|
||||
tar -xzf "$work_dir/$CIRCT_ARCHIVE" -C "$work_dir/circt"
|
||||
|
||||
@@ -11,8 +11,10 @@ APP_FLASH_OFFSET ?= 0x06000000
|
||||
APP ?= hello_uart
|
||||
GPS_DEBUG ?= 1
|
||||
GPS_DEBUG_LEVEL ?= 1
|
||||
GPS_ESKF_ENABLE ?= 1
|
||||
IMU_SENSOR ?= auto
|
||||
BSP_UART_CLOCK_HZ ?= 100000000
|
||||
BSP_UART_BAUDRATE ?= 115200
|
||||
BSP_UART_BAUDRATE ?= 460800
|
||||
GPS_SYSTEM_UART_BAUDRATE ?= 460800
|
||||
BSP_CLINT_TIMEBASE_HZ ?= 1000000
|
||||
GPS_SYSTEM_CLINT_TIMEBASE_HZ ?= 500000
|
||||
@@ -42,6 +44,8 @@ help:
|
||||
@echo " BSP_UART_CLOCK_HZ=$(BSP_UART_CLOCK_HZ)"
|
||||
@echo " BSP_UART_BAUDRATE=$(BSP_UART_BAUDRATE)"
|
||||
@echo " GPS_SYSTEM_UART_BAUDRATE=$(GPS_SYSTEM_UART_BAUDRATE)"
|
||||
@echo " GPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE)"
|
||||
@echo " IMU_SENSOR=$(IMU_SENSOR)"
|
||||
@echo " BSP_CLINT_TIMEBASE_HZ=$(BSP_CLINT_TIMEBASE_HZ)"
|
||||
@echo " GPS_SYSTEM_CLINT_TIMEBASE_HZ=$(GPS_SYSTEM_CLINT_TIMEBASE_HZ)"
|
||||
@echo " GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES=$(GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES)"
|
||||
@@ -63,11 +67,11 @@ steval-mki217v1-test:
|
||||
@make -C steval_mki217v1_test TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/steval_mki217v1_test" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
|
||||
|
||||
gps-system:
|
||||
@make -C gps_system TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/gps_system" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(GPS_SYSTEM_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(GPS_SYSTEM_CLINT_TIMEBASE_HZ)" IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES="$(GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
|
||||
@make -C gps_system TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/gps_system" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" GPS_ESKF_ENABLE="$(GPS_ESKF_ENABLE)" IMU_SENSOR="$(IMU_SENSOR)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(GPS_SYSTEM_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(GPS_SYSTEM_CLINT_TIMEBASE_HZ)" IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES="$(GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
|
||||
|
||||
app:
|
||||
@test -d "$(APP)"
|
||||
@make -C "$(APP)" TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/$(APP)" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
|
||||
@make -C "$(APP)" TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/$(APP)" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" GPS_ESKF_ENABLE="$(GPS_ESKF_ENABLE)" IMU_SENSOR="$(IMU_SENSOR)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
|
||||
|
||||
flash-image: app
|
||||
@mkdir -p "$(FLASH_DIR)"
|
||||
|
||||
@@ -13,7 +13,7 @@ APP_FLASH_OFFSET ?= 0x06000000
|
||||
MARCH ?= rv64imafdc_zicsr_zifencei
|
||||
MABI ?= lp64d
|
||||
BSP_UART_CLOCK_HZ ?= 100000000
|
||||
BSP_UART_BAUDRATE ?= 115200
|
||||
BSP_UART_BAUDRATE ?= 460800
|
||||
BSP_DTIM_BASE ?= 0x80000000
|
||||
BSP_DTIM_SIZE ?= 0x00100000
|
||||
CONFIG_STAMP := $(BUILD_DIR)/build_config.txt
|
||||
|
||||
@@ -7,7 +7,7 @@
|
||||
#endif
|
||||
|
||||
#ifndef BSP_UART_BAUDRATE
|
||||
#define BSP_UART_BAUDRATE 115200UL
|
||||
#define BSP_UART_BAUDRATE 460800UL
|
||||
#endif
|
||||
|
||||
void bsp_init(void)
|
||||
|
||||
@@ -13,7 +13,7 @@ LINKER_SCRIPT ?= $(COMMON_DIR)/linker_ram.ld
|
||||
MARCH ?= rv64imafdc_zicsr_zifencei
|
||||
MABI ?= lp64d
|
||||
BSP_UART_CLOCK_HZ ?= 100000000
|
||||
BSP_UART_BAUDRATE ?= 115200
|
||||
BSP_UART_BAUDRATE ?= 460800
|
||||
BSP_CLINT_TIMEBASE_HZ ?= 1000000
|
||||
BSP_DTIM_BASE ?= 0x80000000
|
||||
BSP_DTIM_SIZE ?= 0x00100000
|
||||
|
||||
@@ -6,6 +6,22 @@ Bu klasör Genesys2 kartından UART ile alınan GPS WLS konumlarını ve PMOD SP
|
||||
ve jiroskop biası. Kartın durağan olduğu deneyde zero-velocity update (ZUPT)
|
||||
uygulanır ve GPS innovation değerlerine aykırı-ölçüm kapısı konur.
|
||||
|
||||
Bare-metal ürün firmware'i ana dizinden iki profille üretilebilir:
|
||||
|
||||
```sh
|
||||
# TDK + GPS + ESKF (varsayılan GPS_ESKF_ENABLE=1)
|
||||
make FPGA_BOARD=genesys2 APP=gps_system \
|
||||
GPS_ESKF_ENABLE=1 IMU_SENSOR=tdk fpga-mcs
|
||||
|
||||
# GPS-only; IMU/ESKF kapalı, WLS/Doppler/PPS açık
|
||||
make FPGA_BOARD=genesys2 APP=gps_system \
|
||||
GPS_ESKF_ENABLE=0 fpga-mcs
|
||||
```
|
||||
|
||||
Firmware ve UART kayıt aracının varsayılan hızı `460800` baud'dur. İki profil
|
||||
aynı MCS/BIN çıktı yolunu kullandığı için ikinci build ilk dosyanın üzerine
|
||||
yazar.
|
||||
|
||||
## Güncel durum ve devralma notu (2026-07-16)
|
||||
|
||||
Bu bölüm yeni bir geliştirme oturumunun nereden devam edeceğini tek yerde
|
||||
@@ -71,6 +87,62 @@ GPS position + Doppler velocity --\
|
||||
TDK veya ST IMU -> imu_sensor ----/
|
||||
```
|
||||
|
||||
### Doppler ESKF'ye nasıl geliyor?
|
||||
|
||||
1. Tracking RTL her 20 ms NAV-bit epoch'unda carrier NCO Q7 değerini,
|
||||
`ABS_SAMPLE`, remaining code phase ve 20 prompt'un I/Q/WBP toplamlarını
|
||||
birlikte latch eder. Payload 20 ms sabit kalır; APB wrapper registered
|
||||
synchronizer ve settle süresinden sonra atomik register bankı ile interrupt
|
||||
üretir.
|
||||
2. Firmware coherent mailbox okumasında sequence'i payload öncesi/sonrası
|
||||
kontrol eder. Carrier değeri için
|
||||
`doppler_hz = carrier_q7/128 - 4092000`, L1 pseudorange-rate için
|
||||
`range_rate = -doppler_hz*c/1575420000` kullanılır.
|
||||
3. Aynı PRN'in 50 Hz snapshot'larında C/N0 `alpha=0.2` ile yumuşatılır.
|
||||
Doppler'in ham değeri ile `alpha=0.05` software-IIR değeri ayrı tutulur;
|
||||
RTL hardware Doppler IIR içermez.
|
||||
4. Ham Doppler, en az dört uyduyla C/N0-weighted velocity WLS
|
||||
`[receiver_vx, receiver_vy, receiver_vz, receiver_clock_drift_mps]`
|
||||
state'ini çözer. Her observation'dan satellite LOS velocity çıkarılır,
|
||||
broadcast satellite clock drift eklenir. Inverse normal matrix ve post-fit
|
||||
residual ECEF velocity covariance'ini üretir. En az altı kullanılabilir
|
||||
uyduda `3.5 sigma` normalized-residual gate açılırsa bütün tek-kanal
|
||||
leave-one-out adayları denenir ve en düşük weighted residual'lı aday ESKF
|
||||
yolu için seçilir.
|
||||
5. GPS adapter velocity ve 3x3 covariance'i ECEF'ten ENU'ya döndürür,
|
||||
kart ölçümüne dayalı minimum `2/2/3 m/s` standard-deviation floor'larını
|
||||
uygular ve position ile aynı capture timestamp'ini korur.
|
||||
6. Fusion engine gecikmiş position ve velocity'yi aynı geçmiş epoch'unda
|
||||
uygular. Velocity'nin ayrı 3-DOF `%99.9`, `NIS=16.266` kapısı vardır; bir
|
||||
velocity reject aynı epoch'taki kabul edilmiş position update'ini bozmaz.
|
||||
Sonra daha yeni IMU history current-time horizon'a kadar replay edilir.
|
||||
|
||||
Filtreli Doppler ikinci bir velocity WLS'e girer; bu sonuç fusion/ESKF'ye
|
||||
yayınlanmaz. Yalnız PPS yazılım filtresinde position-WLS referans datumunun
|
||||
clock-bias drift predictor'u ve PPS kalite metriği olarak kullanılır. PPS'in
|
||||
ana ölçümü position WLS `clock_bias_m` değeridir. Doppler WLS'nin dördüncü
|
||||
`receiver_clock_drift_mps` state'i PPS'e doğrudan verilmez; filtreli receiver
|
||||
velocity ve referans uydu ephemeris hızıyla datum türevi ayrıca hesaplanır.
|
||||
|
||||
Klasik mutlak-pseudorange modelinde receiver clock bias
|
||||
`c*delta_t_r`, Doppler clock drift ise bunun türevi
|
||||
`c*d(delta_t_r)/dt`'dir. Doppler WLS carrier pseudorange-rate'ten
|
||||
`[vx,vy,vz,receiver_clock_drift]` çözer. `0.1 ppm` receiver oscillator
|
||||
error'i yaklaşık `29.98 m/s` ortak Doppler drift'idir; dördüncü state
|
||||
çözülmezse bunun bir bölümü platform hızı sanılır.
|
||||
|
||||
Bu projedeki position WLS bias'i ise capture'lar ortak receiver zamanına göre
|
||||
farklandığı ve `STARTOFFSET` eklendiği için saf receiver clock bias'i değil,
|
||||
referans-uyduya bağlı göreli bir datumdur. Bu yüzden position
|
||||
`clock_bias_m` ile Doppler `clock_drift_mps` arasında doğrudan türev kısıtı
|
||||
kurulmaz. Doppler WLS ayrı bir normal equation kullanır fakat LOS vektörleri
|
||||
için güncel position WLS receiver konumuna, uydu konum/hız/clock drift'i için
|
||||
broadcast ephemerise bağlıdır.
|
||||
|
||||
Bu zincirin RTL/register ayrıntısı
|
||||
`hardware/external/gps/rtl/trck_src/README.md`, bare-metal WLS ve kart
|
||||
doğrulaması `soc/software/gps_system/README.md` içindedir.
|
||||
|
||||
Python tarafındaki giriş noktaları:
|
||||
|
||||
- `serial_capture.py`: UART akışını zaman damgalı CSV olarak toplar;
|
||||
@@ -149,11 +221,12 @@ konum çözülmesi tek başına PPS üretildiğini kanıtlamaz.
|
||||
|
||||
İlk kabul edilen WLS/PPS commit'i yalnız coarse alignment yapar. Ardından faz
|
||||
hatası en fazla sekiz adet 125 MHz tracking-clock tick'i, yani 64 ns olan beş
|
||||
ardışık PI update gerekir. Lock kazanıldıktan sonra 256 ns üzerindeki yeni bir
|
||||
faz hatası lock'u düşürür. WLS receiver-clock bias/pseudorange jitter'ı veya
|
||||
epoch capture belirsizliği beş ardışık ölçümde 64 ns koşulunu bozarsa konum
|
||||
gelmeye devam ederken PPS pini sürekli LOW kalabilir. Bu şu an en güçlü
|
||||
olasılıktır; register ölçmeden eşik değiştirilmemelidir.
|
||||
ardışık PI update gerekir. RTL reset release eşiği 256 ns'dir; v2.2 production
|
||||
software profile'ları tek seyrek WLS sıçraması pulse kaybettirmesin diye
|
||||
release eşiğini runtime `LOOP_CONFIG` ile 1.024 us yapar. WLS receiver-clock
|
||||
bias/pseudorange jitter'ı veya epoch capture belirsizliği beş ardışık ölçümde
|
||||
64 ns acquire koşulunu bozarsa konum gelmeye devam ederken PPS pini LOW
|
||||
kalabilir.
|
||||
|
||||
Firmware ancak WLS başarılı, `solution_valid=true`, PPS referans satırı FDE ile
|
||||
dışlanmamış ve referans kanalın capture sequence'i sıfır değilse PPS mailbox'a
|
||||
@@ -185,7 +258,7 @@ monitor resume
|
||||
|
||||
| Register | Beklenen/yorum |
|
||||
|---|---|
|
||||
| `VERSION @ 0x1002a07c` | `0x00020100`; farklıysa ELF/bitstream eşleşmiyor |
|
||||
| `VERSION @ 0x1002a07c` | Production v2.2 için `0x00020200`; farklıysa ELF/bitstream eşleşmesini kontrol edin |
|
||||
| `CONTROL @ 0x1002a000` | Enable + track açıkken alt bitler genellikle `0x3` |
|
||||
| `STATUS @ 0x1002a004` | `0x1`: commit/alignment yok; alt bitler `0x7`: aligned fakat lock değil; bit 11 set ve alt bitler `0xf`: locked/valid |
|
||||
| `ACTIVE_SEQUENCE @ 0x1002a048` | `0`: firmware hiç geçerli PPS commit'i yapmamış; artıyorsa WLS referansları PPS'ye ulaşıyor |
|
||||
@@ -197,7 +270,7 @@ lock acquisition/faz jitter'ıdır. `STATUS` lock gösteriyor ve `PULSE_COUNT`
|
||||
artıyorsa JC10 sinyali JC11 GND referansıyla osiloskop veya logic analyzer'da
|
||||
ölçülmelidir; pulse saniyede bir yaklaşık 100 ms HIGH'dır.
|
||||
|
||||
2026-07-25 canlı Genesys2 ölçümünde `VERSION=0x00020100`,
|
||||
Erken 2026-07-25 v2.1 canlı Genesys2 ölçümünde `VERSION=0x00020100`,
|
||||
`CONTROL=0x00000003` ve snapshot `ACTIVE_SEQUENCE=11583` görüldü.
|
||||
`PULSE_COUNT=314`, PPS core'un resetten beri 314 `pps_out` pulse olayı
|
||||
oluşturduğunu kanıtladı; post-route top-level yol da sabit LOW değildir.
|
||||
@@ -218,6 +291,13 @@ işaret eder. Eşik ölçümsüz büyütülmemeli; önce clock-bias/TDOP kalite
|
||||
outlier reddi veya faz gözlemi süzme stratejisi board A/B testiyle
|
||||
değerlendirilmelidir.
|
||||
|
||||
Bu tarihsel v2.1 teşhisinden sonra v2.2 dinamik PI profilleri, Doppler
|
||||
clock-drift prediction, bounded WLS clock-bias filtresi, 2 s validity
|
||||
watchdog/30 s state koruma politikası ve fractional capture correction
|
||||
eklenmiştir. Güncel sentetik ve kart sonuçları
|
||||
`soc/software/gps_system/README.md` altındaki PPS bölümlerinde, register/RTL
|
||||
davranışı `hardware/external/gps/rtl/pps/README.md` içinde tutulur.
|
||||
|
||||
## Çalıştırma
|
||||
|
||||
UART kaydı:
|
||||
|
||||
@@ -14,7 +14,7 @@ import serial
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--port", default="/dev/ttyUSB2")
|
||||
parser.add_argument("--baud", type=int, default=115200)
|
||||
parser.add_argument("--baud", type=int, default=460800)
|
||||
parser.add_argument("--duration", type=float, default=60.0)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument(
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
.venv/
|
||||
@@ -0,0 +1,241 @@
|
||||
# GPS code measurement ve DLL kalite modeli
|
||||
|
||||
Bu dizin, mevcut firmware/RTL ölçüm yolunu değiştirmeden önce code-phase
|
||||
hassasiyeti, DLL jitter telemetrisi, dinamik WLS sigma ve carrier/Doppler
|
||||
smoothing seçeneklerini aynı sentetik veri üzerinde karşılaştırır.
|
||||
|
||||
Bu genel amaçlı bir RF/GNSS simülatörü değildir. Projeye özgü executable
|
||||
tasarım modelidir.
|
||||
|
||||
Dizin iki ayrı kararı inceler:
|
||||
|
||||
1. Position WLS için C/N0'ın açıklamadığı DLL jitter/variance telemetrisi.
|
||||
2. Velocity WLS ve ESKF öncesindeki sabit Doppler IIR'in gürültü-gecikme
|
||||
etkisi.
|
||||
|
||||
## Modellenen mevcut mimari
|
||||
|
||||
| Proje davranışı | Model |
|
||||
|---|---|
|
||||
| Normalized early-minus-late DLL discriminator | `rtl_dll_model.rtl_discriminator()` |
|
||||
| Signed Q2.13 discriminator quantization | `rtl_dll_model.py` |
|
||||
| `LF_code.sv` C1=`21.25`, C2=`0.05712890625` ve Q20 çıkış | `CodeLoopFilterRtl` |
|
||||
| 20 adet 1 ms DLL örneği/NAV bit | `synthesize_dll_window()` |
|
||||
| Firmware C/N0 IIR, alpha=`0.2` | `ChannelQuality.update()` |
|
||||
| URA/elevation/C/N0 position sigma | `firmware_sigma_m(CURRENT)` |
|
||||
| ECEF x/y/z + common clock-bias WLS | `firmware_wls()` |
|
||||
| 99% chi-square ve en az 7 uyduda leave-one-out FDE | `firmware_wls()` |
|
||||
| Position solve periyodu 200 ms | `measurement_scenarios.py` |
|
||||
| Doppler IIR alpha=`0.05`, 50 Hz | `doppler_eskf_model.py` |
|
||||
| ECEF velocity + common clock-drift WLS | `velocity_wls()` |
|
||||
| Velocity WLS periyodu 200 ms | `doppler_eskf_model.py` |
|
||||
| TDK 100 Hz IMU process-noise profili | `VelocityBiasEskf` |
|
||||
| Velocity 3-DOF `%99.9`, NIS=`16.266236` | `VelocityBiasEskf.update_velocity()` |
|
||||
|
||||
Kaynak eşlemeleri:
|
||||
|
||||
- `hardware/external/gps/rtl/trck_src/code_loop_discriminator.sv`
|
||||
- `hardware/external/gps/rtl/trck_src/LF_code.sv`
|
||||
- `hardware/external/gps/rtl/trck_src/NCO_code.sv`
|
||||
- `hardware/external/gps/rtl/trck_src/tracking_measurement.sv`
|
||||
- `hardware/external/gps/rtl/common/definitions.sv`
|
||||
- `soc/software/gps_system/src/gps/gps_tracking_measurement.c`
|
||||
- `soc/software/gps_system/src/gps/gps_interrupt_handlers.c`
|
||||
- `soc/software/gps_system/src/gps/NavEquations.c`
|
||||
|
||||
`REM_CODE_PHASE`, DLL discriminator residual'ı olarak modellenmez. RTL'de bu
|
||||
değer code NCO'nun epoch taşma remainder'ıdır. Yeni kalite metriğinin girdisi
|
||||
RTL içindeki gerçek signed `dll_error` örnekleridir.
|
||||
|
||||
## Karşılaştırılan sigma politikaları
|
||||
|
||||
- `current_cn0`: mevcut URA + elevation + C/N0 modeli.
|
||||
- `naive_add_dll`: DLL variance'ı bağımsızmış gibi ekler. Nominal durumda aynı
|
||||
thermal jitter'ı iki kere saydığı için negatif kontrol olarak bulunur.
|
||||
- `dll_excess`: yalnız ölçülen DLL variance'ın C/N0'dan beklenen variance'ı aşan
|
||||
bölümünü code sigma'ya ekler.
|
||||
- `dll_excess_guard`: excess variance'a ek olarak sürekli discriminator mean
|
||||
stress'inde ağırlığı azaltır.
|
||||
- `oracle_variance`: sentetik gerçek jitter çarpanını bilir. Ulaşılabilir üst
|
||||
sınırı gösterir; firmware algoritması değildir.
|
||||
|
||||
Dinamik model:
|
||||
|
||||
```text
|
||||
sigma_base^2 = sigma_URA^2 + sigma_elevation^2 + sigma_CN0^2
|
||||
ratio = EWMA(measured_DLL_variance / expected_DLL_variance(C/N0))
|
||||
|
||||
sigma_dynamic^2 =
|
||||
sigma_URA^2
|
||||
+ sigma_elevation^2
|
||||
+ sigma_CN0^2 * max(1, ratio)
|
||||
```
|
||||
|
||||
Bu form C/N0 ile DLL thermal jitter'ını iki kere toplamaz. Yalnızca C/N0
|
||||
modelinin açıklayamadığı kanal-bağımlı excess jitter'ı şişirir.
|
||||
|
||||
## Senaryolar
|
||||
|
||||
- Temiz açık gökyüzü
|
||||
- Bütün kanallarda düşük C/N0
|
||||
- C/N0'da görünmeyen tek-kanal DLL jitter
|
||||
- Bütün kanallarda ortak, C/N0'da görünmeyen DLL jitter
|
||||
- Gürültü burst ve recovery
|
||||
- Re-lock sırasında DLL mean/jitter stress
|
||||
- Düşük-variance stable multipath bias
|
||||
- Bias + excess jitter
|
||||
- Kötü geometri + gizli jitter
|
||||
- İlk başta düşük, sonra iyileşen C/N0
|
||||
|
||||
Ayrı 1D model ham code, kör scalar IIR ve Doppler-predicted code smoothing'i
|
||||
range-rate adımları ve residual carrier CFO altında karşılaştırır. Carrier
|
||||
ölçümleri RTL cadence'i olan 50 Hz'de üretilir ve 200 ms WLS epoch'una kadar
|
||||
entegre edilir. Ham, sabit `alpha=0.05` ve IMU hareket durumuna göre bant
|
||||
genişliğini açan adaptif Doppler yolları birlikte denenir. Bu ayrım önemlidir:
|
||||
PPS datum-drift predictor'u için yararlı olan yavaş Doppler filtresi, ESKF
|
||||
velocity veya hızlı code-aiding prediction'ında dinamik lag üretebilir; tam
|
||||
ham snapshot ise kartta ölçülen gürültü seviyesinde fazla varyans üretebilir.
|
||||
|
||||
## Doppler pre-IIR ve ESKF modeli
|
||||
|
||||
`doppler_eskf_model.py`, aynı truth, tracking gürültüsü ve IMU örnekleriyle şu
|
||||
dört yolu karşılaştırır:
|
||||
|
||||
- Önceki: 50 Hz `alpha=0.05` IIR -> 5 Hz velocity WLS -> ESKF.
|
||||
- Ham Doppler -> 5 Hz velocity WLS -> ESKF.
|
||||
- C/N0 + düşük-geçiren navigation acceleration ile adaptif IIR -> WLS -> ESKF.
|
||||
- Ham Doppler -> RAIM-benzeri leave-one-out robust WLS -> ESKF.
|
||||
|
||||
Mevcut `alpha=0.05` tek-kutuplu IIR için 50 Hz'de eşdeğer cutoff yaklaşık
|
||||
`0.408 Hz`, zaman sabiti `0.390 s`, DC group delay ise yaklaşık `380 ms`'dir.
|
||||
Bu gecikme constant velocity'de görünmez; acceleration sırasında velocity
|
||||
measurement innovation'ına doğrudan girer.
|
||||
|
||||
ESKF modeli bu karar için gerekli `[velocity, accel-bias]` alt sistemidir.
|
||||
Firmware'deki TDK process acceleration noise density `0.025
|
||||
m/s2/sqrt(Hz)`, accelerometer bias random walk `0.0015 m/s2/sqrt(s)`, WLS'nin
|
||||
gerçek 3x3 covariance'ı, fusion adapter'ın `2/2/3 m/s` ENU velocity sigma
|
||||
tabanları ve velocity NIS kapısı kullanılır. Position, attitude, lever-arm ve
|
||||
tam 15-state cross-covariance bu modelin kapsamında değildir.
|
||||
|
||||
Senaryo matrisi sabit uçuş, airliner/aggressive manevra, düşük C/N0, tek-kanal
|
||||
Doppler burst, common receiver clock-drift step, 15 saniye GPS kesintisi ve
|
||||
birleşik arıza koşullarını içerir.
|
||||
|
||||
Nominal 20-seed sentetik sonuçların özeti:
|
||||
|
||||
- ESKF ham WLS gürültüsünü nominalde yaklaşık `0.416 m/s` 3D RMS'ten `0.190
|
||||
m/s` seviyesine indirir; temporal filtreleme zaten ESKF'de gerçekleşir.
|
||||
- Sabit pre-IIR sakin uçuşta `0.182 m/s`, ham yol `0.190 m/s` verir. Airliner
|
||||
manevrasında sabit IIR gecikmesi sonucu `0.416 m/s`, ham yol `0.190 m/s`;
|
||||
aggressive manevrada sırasıyla `0.942 m/s` ve `0.190 m/s` oluşur. Adapter
|
||||
sigma tabanları iki yolu da NIS içinde tutsa bile dinamik lag kalır.
|
||||
- Düşük C/N0 manevrasında adaptif taslak en iyi gürültü/gecikme uzlaşmasını
|
||||
verir; eşikleri firmware'e taşımadan önce kart loguyla kalibre etmek gerekir.
|
||||
- ESKF yalnız aggregate 3D velocity gördüğünden tek uydu burst'ünün kaynağını
|
||||
ayıramaz. Kanal outlier/FDE işlemi velocity WLS öncesinde yapılmalıdır.
|
||||
- Common receiver clock-frequency hatası WLS'nin dördüncü clock-drift state'i
|
||||
tarafından hızdan ayrılır.
|
||||
|
||||
Genesys2 sabit-kart ölçümünde tam ham yolun velocity-WLS RMS'i `9.17 m/s`,
|
||||
p95'i `16.71 m/s` ve FDE oranı `86/120` oldu. Aynı epoch'larda sabit
|
||||
`alpha=0.05` yol `0.92 m/s` RMS, `1.63 m/s` p95 ve `0/120` FDE verdi. Bu
|
||||
nedenle nominal modelde iyi görünen doğrudan ham yol production'a alınmadı.
|
||||
Kart gürültüsünü eşlemek için tracking noise `22x` ile kalibre edilen regresyon
|
||||
modelindeki regresyon seed'leri için steady-7/airliner-3/aggressive-7 fused
|
||||
RMS değerleri:
|
||||
|
||||
- sabit IIR: `0.451 / 0.484 / 1.036 m/s`,
|
||||
- adaptif IIR: `0.451 / 0.336 / 0.538 m/s`,
|
||||
- tam ham: `1.260 / 1.100 / 1.260 m/s`.
|
||||
|
||||
Production firmware ESKF için `alpha=0.05/0.20/0.40` hareket-adaptif Doppler
|
||||
-> bandwidth-aware sigma -> kanal validity/FDE -> velocity WLS + covariance
|
||||
-> ESKF yolunu kullanır. Ham alan tanısal kayıt ve offline analiz için korunur.
|
||||
Ayrı sabit `alpha=0.05` Doppler velocity yalnız PPS position-WLS clock-bias
|
||||
datumunun drift predictor'u ve PPS kalite metriği için kullanılır. Ayrıntılı
|
||||
nominal tablo ve model sınırları `output/DOPPLER_ESKF_ANALYSIS.md` içindedir;
|
||||
kart-kalibreli karar `test_doppler_eskf_model.py` ile regresyona alınmıştır.
|
||||
|
||||
## Kurulum ve çalıştırma
|
||||
|
||||
Repo içindeki `soc/software` dizininden:
|
||||
|
||||
```bash
|
||||
python3 -m venv gps_measurement_python/.venv
|
||||
gps_measurement_python/.venv/bin/pip install \
|
||||
-r gps_measurement_python/requirements.txt
|
||||
gps_measurement_python/.venv/bin/python -m unittest \
|
||||
gps_measurement_python/test_measurement_model.py \
|
||||
gps_measurement_python/test_doppler_eskf_model.py
|
||||
gps_measurement_python/.venv/bin/python \
|
||||
-m gps_measurement_python.analyze_measurement_scenarios \
|
||||
--seeds 12 --jobs 4
|
||||
gps_measurement_python/.venv/bin/python \
|
||||
-m gps_measurement_python.analyze_doppler_eskf \
|
||||
--seeds 20 --jobs 4
|
||||
```
|
||||
|
||||
Sistem Python'unda `venv` paketi yoksa bağımlılıklar izole bir target dizinine
|
||||
de kurulabilir:
|
||||
|
||||
```bash
|
||||
python3 -m pip install --target /tmp/gps-measurement-deps \
|
||||
-r gps_measurement_python/requirements.txt
|
||||
PYTHONPATH=/tmp/gps-measurement-deps:. python3 \
|
||||
-m unittest gps_measurement_python/test_measurement_model.py
|
||||
PYTHONPATH=/tmp/gps-measurement-deps:. python3 \
|
||||
-m gps_measurement_python.analyze_measurement_scenarios \
|
||||
--seeds 12 --jobs 4
|
||||
```
|
||||
|
||||
Çıktılar `gps_measurement_python/output/` altındadır:
|
||||
|
||||
- `scenario_summary.csv`
|
||||
- `ANALYSIS.md`
|
||||
- `01_position_3d_rms.png`
|
||||
- `02_solution_availability.png`
|
||||
- `03_noise_burst_transition.png`
|
||||
- `04_code_smoothing_cfo.png`
|
||||
- `doppler_eskf_summary.csv`
|
||||
- `DOPPLER_ESKF_ANALYSIS.md`
|
||||
- `05_doppler_eskf_velocity_rms.png`
|
||||
- `06_airliner_maneuver_trace.png`
|
||||
- `07_outlier_wls_nis.png`
|
||||
- `08_low_cn0_wls_vs_eskf.png`
|
||||
|
||||
## Kalibrasyon sınırı
|
||||
|
||||
Modelde C/N0'dan beklenen normalized discriminator standard deviation'ı
|
||||
45 dB-Hz için `0.020` kabul edilir ve firmware ile aynı 20 dB amplitude yasası
|
||||
ile ölçeklenir. Bu sayı karttan alınmış bir kalibrasyon değildir.
|
||||
|
||||
Karar vermeden önce karttan her kanal için en az şu 50 Hz kayıt alınmalıdır:
|
||||
|
||||
```text
|
||||
sequence, PRN, C/N0,
|
||||
DLL mean, DLL variance, DLL sample count,
|
||||
code NCO correction, carrier frequency,
|
||||
ABS_SAMPLE, REM_CODE_PHASE, lock
|
||||
```
|
||||
|
||||
Kayıtlarla `expected_DLL_variance(C/N0)` eğrisi çıkarılmalı; temperature,
|
||||
elevation ve lock yaşına göre ayrıştırılmalıdır. Stable multipath, DLL yeni
|
||||
denge noktasına oturduğunda düşük variance bırakabilir. Bu durum tek başına
|
||||
DLL variance ile çözülemez; ek correlator tap'leri veya correlation-shape
|
||||
metriği gerekir.
|
||||
|
||||
RTL için en düşük maliyetli keşif arayüzü, her 20 ms NAV-bit penceresinde
|
||||
`dll_error` üzerinden aşağıdaki ham istatistikleri biriktirmektir:
|
||||
|
||||
```text
|
||||
signed sum(error) : 15 + ceil(log2(20)) = 20 bit
|
||||
unsigned sum(error^2) : 30 + ceil(log2(20)) = 35 bit
|
||||
valid sample count : 5 bit
|
||||
```
|
||||
|
||||
35-bit kare toplamı APB'de iki 32-bit register ile taşınabilir. Ortalama,
|
||||
sample variance, C/N0 expectation ratio ve hysteresis yazılımda hesaplanır.
|
||||
Bölme ve square-root RTL'ye konmaz. Bu tuple mevcut measurement sequence,
|
||||
C/N0, carrier frequency, `ABS_SAMPLE` ve `REM_CODE_PHASE` ile aynı 20 ms
|
||||
snapshot'ta tutulmalıdır.
|
||||
@@ -0,0 +1 @@
|
||||
"""Project-specific GPS code-measurement and WLS analysis model."""
|
||||
@@ -0,0 +1,428 @@
|
||||
"""Monte Carlo comparison of Doppler prefilter choices before velocity ESKF."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import math
|
||||
from concurrent.futures import ProcessPoolExecutor
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from .doppler_eskf_model import (
|
||||
DopplerEskfTrace,
|
||||
DopplerPath,
|
||||
PATH_ORDER,
|
||||
VELOCITY_NIS_GATE,
|
||||
flight_scenarios,
|
||||
run_doppler_eskf_scenario,
|
||||
)
|
||||
|
||||
|
||||
PATH_LABEL = {
|
||||
DopplerPath.FIXED_IIR: "Önceki sabit IIR",
|
||||
DopplerPath.RAW: "Ham Doppler + ESKF",
|
||||
DopplerPath.ADAPTIVE_IIR: "Adaptif IIR + ESKF",
|
||||
DopplerPath.RAW_ROBUST: "Ham + robust WLS + ESKF",
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TrialMetrics:
|
||||
scenario: str
|
||||
path: DopplerPath
|
||||
fused_rms_mps: float
|
||||
fused_p95_mps: float
|
||||
quiet_rms_mps: float
|
||||
dynamic_rms_mps: float
|
||||
wls_rms_mps: float
|
||||
wls_p95_mps: float
|
||||
clock_drift_rms_mps: float
|
||||
velocity_accept_percent: float
|
||||
nis_reject_percent: float
|
||||
fused_95_coverage_percent: float
|
||||
excluded_epoch_percent: float
|
||||
mean_covariance_scale: float
|
||||
mean_alpha: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Summary:
|
||||
scenario: str
|
||||
path: DopplerPath
|
||||
seeds: int
|
||||
fused_rms_mps: float
|
||||
fused_rms_seed_std_mps: float
|
||||
fused_p95_mps: float
|
||||
quiet_rms_mps: float
|
||||
dynamic_rms_mps: float
|
||||
wls_rms_mps: float
|
||||
wls_p95_mps: float
|
||||
clock_drift_rms_mps: float
|
||||
velocity_accept_percent: float
|
||||
nis_reject_percent: float
|
||||
fused_95_coverage_percent: float
|
||||
excluded_epoch_percent: float
|
||||
mean_covariance_scale: float
|
||||
mean_alpha: float
|
||||
|
||||
|
||||
def _rms(values: np.ndarray) -> float:
|
||||
return float(np.sqrt(np.mean(np.square(values)))) if len(values) else math.nan
|
||||
|
||||
|
||||
def _truth_at_wls(trace: DopplerEskfTrace) -> np.ndarray:
|
||||
return np.column_stack(
|
||||
[
|
||||
np.interp(
|
||||
trace.wls_time_s,
|
||||
trace.time_s,
|
||||
trace.truth_velocity_mps[:, axis],
|
||||
)
|
||||
for axis in range(3)
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def trial_metrics(scenario_index: int, trace: DopplerEskfTrace) -> TrialMetrics:
|
||||
scenario = flight_scenarios()[scenario_index]
|
||||
burn_in = trace.time_s >= 5.0
|
||||
fused_vector_error = trace.fused_velocity_mps - trace.truth_velocity_mps
|
||||
fused_error = np.linalg.norm(fused_vector_error, axis=1)
|
||||
dynamic = burn_in & (np.linalg.norm(trace.acceleration_mps2, axis=1) > 0.25)
|
||||
quiet = burn_in & ~dynamic
|
||||
|
||||
fused_nees = []
|
||||
for sample in np.flatnonzero(burn_in):
|
||||
covariance = trace.fused_velocity_covariance_mps2[sample]
|
||||
error = fused_vector_error[sample]
|
||||
fused_nees.append(float(error @ np.linalg.inv(covariance) @ error))
|
||||
fused_nees_array = np.asarray(fused_nees)
|
||||
|
||||
wls_error = np.linalg.norm(trace.wls_velocity_mps - _truth_at_wls(trace), axis=1)
|
||||
true_clock_drift = np.asarray(
|
||||
[scenario.receiver_clock_drift_mps(float(time)) for time in trace.wls_time_s]
|
||||
)
|
||||
clock_error = trace.wls_clock_drift_mps - true_clock_drift
|
||||
valid_nis = trace.wls_nis[np.isfinite(trace.wls_nis)]
|
||||
|
||||
return TrialMetrics(
|
||||
scenario=trace.scenario,
|
||||
path=trace.path,
|
||||
fused_rms_mps=_rms(fused_error[burn_in]),
|
||||
fused_p95_mps=float(np.percentile(fused_error[burn_in], 95.0)),
|
||||
quiet_rms_mps=_rms(fused_error[quiet]),
|
||||
dynamic_rms_mps=_rms(fused_error[dynamic]),
|
||||
wls_rms_mps=_rms(wls_error),
|
||||
wls_p95_mps=float(np.percentile(wls_error, 95.0)),
|
||||
clock_drift_rms_mps=_rms(clock_error),
|
||||
velocity_accept_percent=100.0 * float(np.mean(trace.wls_accepted)),
|
||||
nis_reject_percent=100.0 * float(np.mean(valid_nis > VELOCITY_NIS_GATE)),
|
||||
# 7.8147 is the 95-percent chi-square threshold for three states.
|
||||
fused_95_coverage_percent=100.0 * float(np.mean(fused_nees_array <= 7.8147279)),
|
||||
excluded_epoch_percent=100.0
|
||||
* float(np.mean(trace.excluded_satellites >= 0)),
|
||||
mean_covariance_scale=float(np.mean(trace.wls_covariance_scale)),
|
||||
mean_alpha=float(np.mean(trace.adaptive_alpha)),
|
||||
)
|
||||
|
||||
|
||||
def _worker(task: tuple[int, int]) -> tuple[int, int, list[TrialMetrics]]:
|
||||
scenario_index, seed = task
|
||||
scenario = flight_scenarios()[scenario_index]
|
||||
traces = run_doppler_eskf_scenario(scenario, seed)
|
||||
return (
|
||||
scenario_index,
|
||||
seed,
|
||||
[trial_metrics(scenario_index, traces[path]) for path in PATH_ORDER],
|
||||
)
|
||||
|
||||
|
||||
def summarize(items: list[TrialMetrics]) -> Summary:
|
||||
if not items:
|
||||
raise ValueError("cannot summarize an empty trial list")
|
||||
|
||||
def average(field_name: str) -> float:
|
||||
values = np.asarray([getattr(item, field_name) for item in items], dtype=float)
|
||||
finite = values[np.isfinite(values)]
|
||||
return float(np.mean(finite)) if len(finite) else math.nan
|
||||
|
||||
fused_rms = np.asarray([item.fused_rms_mps for item in items])
|
||||
return Summary(
|
||||
scenario=items[0].scenario,
|
||||
path=items[0].path,
|
||||
seeds=len(items),
|
||||
fused_rms_mps=average("fused_rms_mps"),
|
||||
fused_rms_seed_std_mps=float(np.std(fused_rms)),
|
||||
fused_p95_mps=average("fused_p95_mps"),
|
||||
quiet_rms_mps=average("quiet_rms_mps"),
|
||||
dynamic_rms_mps=average("dynamic_rms_mps"),
|
||||
wls_rms_mps=average("wls_rms_mps"),
|
||||
wls_p95_mps=average("wls_p95_mps"),
|
||||
clock_drift_rms_mps=average("clock_drift_rms_mps"),
|
||||
velocity_accept_percent=average("velocity_accept_percent"),
|
||||
nis_reject_percent=average("nis_reject_percent"),
|
||||
fused_95_coverage_percent=average("fused_95_coverage_percent"),
|
||||
excluded_epoch_percent=average("excluded_epoch_percent"),
|
||||
mean_covariance_scale=average("mean_covariance_scale"),
|
||||
mean_alpha=average("mean_alpha"),
|
||||
)
|
||||
|
||||
|
||||
def write_csv(path: Path, summaries: list[Summary]) -> None:
|
||||
with path.open("w", newline="", encoding="utf-8") as output:
|
||||
writer = csv.DictWriter(output, fieldnames=list(Summary.__dataclass_fields__))
|
||||
writer.writeheader()
|
||||
for item in summaries:
|
||||
row = dict(item.__dict__)
|
||||
row["path"] = item.path.value
|
||||
writer.writerow(row)
|
||||
|
||||
|
||||
def _summary_plot(path: Path, summaries: list[Summary]) -> None:
|
||||
scenarios = flight_scenarios()
|
||||
x = np.arange(len(scenarios))
|
||||
width = 0.19
|
||||
figure, axis = plt.subplots(figsize=(15, 6.3))
|
||||
for path_index, doppler_path in enumerate(PATH_ORDER):
|
||||
values = [
|
||||
next(
|
||||
item.fused_rms_mps
|
||||
for item in summaries
|
||||
if item.scenario == scenario.name and item.path == doppler_path
|
||||
)
|
||||
for scenario in scenarios
|
||||
]
|
||||
axis.bar(
|
||||
x + (path_index - 1.5) * width,
|
||||
values,
|
||||
width,
|
||||
label=PATH_LABEL[doppler_path],
|
||||
)
|
||||
axis.set_ylabel("ESKF 3D velocity RMS (m/s)")
|
||||
axis.set_yscale("log")
|
||||
axis.set_xticks(x)
|
||||
axis.set_xticklabels([item.name for item in scenarios], rotation=25, ha="right")
|
||||
axis.grid(axis="y", alpha=0.25)
|
||||
axis.legend(ncol=2)
|
||||
figure.tight_layout()
|
||||
figure.savefig(path, dpi=150)
|
||||
plt.close(figure)
|
||||
|
||||
|
||||
def _maneuver_trace_plot(path: Path, trace_set: dict[DopplerPath, DopplerEskfTrace]) -> None:
|
||||
reference = trace_set[DopplerPath.FIXED_IIR]
|
||||
figure, axes = plt.subplots(3, 1, figsize=(13, 8.5), sharex=True)
|
||||
for doppler_path in PATH_ORDER:
|
||||
trace = trace_set[doppler_path]
|
||||
error = np.linalg.norm(trace.fused_velocity_mps - trace.truth_velocity_mps, axis=1)
|
||||
axes[0].plot(trace.time_s, error, label=PATH_LABEL[doppler_path], linewidth=1.0)
|
||||
axes[1].plot(
|
||||
reference.time_s,
|
||||
np.linalg.norm(reference.acceleration_mps2, axis=1),
|
||||
color="black",
|
||||
linewidth=1.2,
|
||||
label="True |acceleration|",
|
||||
)
|
||||
adaptive = trace_set[DopplerPath.ADAPTIVE_IIR]
|
||||
axes[2].step(
|
||||
adaptive.wls_time_s,
|
||||
adaptive.adaptive_alpha,
|
||||
where="post",
|
||||
color="tab:green",
|
||||
)
|
||||
axes[0].set_ylabel("3D velocity error (m/s)")
|
||||
axes[1].set_ylabel("Acceleration (m/s²)")
|
||||
axes[2].set_ylabel("Adaptive alpha")
|
||||
axes[2].set_xlabel("Time (s)")
|
||||
axes[0].set_ylim(bottom=0.0)
|
||||
axes[0].legend(ncol=2)
|
||||
for axis in axes:
|
||||
axis.grid(alpha=0.25)
|
||||
figure.tight_layout()
|
||||
figure.savefig(path, dpi=150)
|
||||
plt.close(figure)
|
||||
|
||||
|
||||
def _outlier_plot(path: Path, trace_set: dict[DopplerPath, DopplerEskfTrace]) -> None:
|
||||
figure, axes = plt.subplots(2, 1, figsize=(13, 7), sharex=True)
|
||||
for doppler_path in PATH_ORDER:
|
||||
trace = trace_set[doppler_path]
|
||||
error = np.linalg.norm(trace.wls_velocity_mps - _truth_at_wls(trace), axis=1)
|
||||
axes[0].plot(trace.wls_time_s, error, label=PATH_LABEL[doppler_path], linewidth=1.0)
|
||||
axes[1].plot(trace.wls_time_s, trace.wls_nis, label=PATH_LABEL[doppler_path], linewidth=0.9)
|
||||
axes[1].axhline(VELOCITY_NIS_GATE, color="black", linestyle="--", linewidth=1.0)
|
||||
for start, stop in ((30.0, 31.0), (56.0, 57.0), (72.0, 72.6)):
|
||||
for axis in axes:
|
||||
axis.axvspan(start, stop, color="tab:red", alpha=0.10)
|
||||
axes[0].set_ylabel("WLS velocity error (m/s)")
|
||||
axes[1].set_ylabel("Velocity NIS")
|
||||
axes[1].set_xlabel("Time (s)")
|
||||
axes[0].legend(ncol=2)
|
||||
for axis in axes:
|
||||
axis.grid(alpha=0.25)
|
||||
figure.tight_layout()
|
||||
figure.savefig(path, dpi=150)
|
||||
plt.close(figure)
|
||||
|
||||
|
||||
def _filtering_plot(path: Path, trace_set: dict[DopplerPath, DopplerEskfTrace]) -> None:
|
||||
figure, axes = plt.subplots(2, 1, figsize=(13, 7), sharex=True)
|
||||
for doppler_path in (DopplerPath.FIXED_IIR, DopplerPath.RAW, DopplerPath.ADAPTIVE_IIR):
|
||||
trace = trace_set[doppler_path]
|
||||
wls_error = np.linalg.norm(trace.wls_velocity_mps - _truth_at_wls(trace), axis=1)
|
||||
fused_error = np.linalg.norm(trace.fused_velocity_mps - trace.truth_velocity_mps, axis=1)
|
||||
axes[0].plot(trace.wls_time_s, wls_error, label=PATH_LABEL[doppler_path], linewidth=0.9)
|
||||
axes[1].plot(trace.time_s, fused_error, label=PATH_LABEL[doppler_path], linewidth=0.9)
|
||||
axes[0].set_ylabel("WLS 3D error (m/s)")
|
||||
axes[1].set_ylabel("ESKF 3D error (m/s)")
|
||||
axes[1].set_xlabel("Time (s)")
|
||||
axes[0].legend(ncol=2)
|
||||
for axis in axes:
|
||||
axis.grid(alpha=0.25)
|
||||
figure.tight_layout()
|
||||
figure.savefig(path, dpi=150)
|
||||
plt.close(figure)
|
||||
|
||||
|
||||
def _lookup(
|
||||
summaries: list[Summary], scenario: str, doppler_path: DopplerPath
|
||||
) -> Summary:
|
||||
return next(
|
||||
item
|
||||
for item in summaries
|
||||
if item.scenario == scenario and item.path == doppler_path
|
||||
)
|
||||
|
||||
|
||||
def write_report(path: Path, summaries: list[Summary], seed_count: int) -> None:
|
||||
lines = [
|
||||
"# Doppler WLS -> ESKF model sonuçları",
|
||||
"",
|
||||
f"Monte Carlo seed sayısı: `{seed_count}`; ilk 5 saniye RMS metriklerinden çıkarıldı.",
|
||||
"50 Hz tracking, önceki `alpha=0.05` Doppler IIR, 5 Hz dört-state velocity WLS, fusion adapter'ın `2/2/3 m/s` ENU sigma tabanları, TDK profiliyle 100 Hz IMU propagation ve 3-DOF `%99.9` NIS kapısı modellenmiştir.",
|
||||
"",
|
||||
"## Sonuç tablosu",
|
||||
"",
|
||||
"| Senaryo | Yol | ESKF RMS (m/s) | Dynamic RMS | WLS RMS | Accept (%) | 95% cov. (%) | Exclude (%) |",
|
||||
"|---|---|---:|---:|---:|---:|---:|---:|",
|
||||
]
|
||||
for scenario in flight_scenarios():
|
||||
for doppler_path in PATH_ORDER:
|
||||
item = _lookup(summaries, scenario.name, doppler_path)
|
||||
dynamic = "-" if not np.isfinite(item.dynamic_rms_mps) else f"{item.dynamic_rms_mps:.3f}"
|
||||
lines.append(
|
||||
f"| {scenario.name} | {PATH_LABEL[doppler_path]} | "
|
||||
f"{item.fused_rms_mps:.3f} | {dynamic} | {item.wls_rms_mps:.3f} | "
|
||||
f"{item.velocity_accept_percent:.1f} | {item.fused_95_coverage_percent:.1f} | "
|
||||
f"{item.excluded_epoch_percent:.1f} |"
|
||||
)
|
||||
|
||||
steady_fixed = _lookup(summaries, "steady_cruise", DopplerPath.FIXED_IIR)
|
||||
steady_raw = _lookup(summaries, "steady_cruise", DopplerPath.RAW)
|
||||
air_fixed = _lookup(summaries, "airliner_maneuver", DopplerPath.FIXED_IIR)
|
||||
air_raw = _lookup(summaries, "airliner_maneuver", DopplerPath.RAW)
|
||||
weak_fixed = _lookup(summaries, "low_cn0_airliner", DopplerPath.FIXED_IIR)
|
||||
weak_raw = _lookup(summaries, "low_cn0_airliner", DopplerPath.RAW)
|
||||
weak_adaptive = _lookup(summaries, "low_cn0_airliner", DopplerPath.ADAPTIVE_IIR)
|
||||
outlier_raw = _lookup(summaries, "single_satellite_outliers", DopplerPath.RAW)
|
||||
outlier_robust = _lookup(summaries, "single_satellite_outliers", DopplerPath.RAW_ROBUST)
|
||||
clock_raw = _lookup(summaries, "clock_drift_steps", DopplerPath.RAW)
|
||||
|
||||
lines.extend(
|
||||
[
|
||||
"",
|
||||
"## Bulgular",
|
||||
"",
|
||||
f"- Sabit hızda pre-IIR ESKF RMS'i `{steady_fixed.fused_rms_mps:.3f}` m/s, ham yol `{steady_raw.fused_rms_mps:.3f}` m/s. Yani pre-IIR sakin durumda daha düzgün çıktı veriyor; ESKF ham gürültüyü zaten `{steady_raw.wls_rms_mps:.3f}` m/s WLS RMS'ten bu seviyeye indiriyor.",
|
||||
f"- Airliner manevrasında sabit IIR `{air_fixed.fused_rms_mps:.3f}` m/s ve `%{air_fixed.nis_reject_percent:.1f}` NIS ret oranı verirken ham yol `{air_raw.fused_rms_mps:.3f}` m/s ve `%{air_raw.nis_reject_percent:.1f}` ret verdi. Konservatif adapter tabanları rejection'ı sınırlasa bile covariance'ın temsil etmediği IIR gecikmesi fused velocity'de dinamik hata bırakır.",
|
||||
f"- Düşük C/N0 manevrasında sabit/ham/adaptif RMS sırasıyla `{weak_fixed.fused_rms_mps:.3f}`, `{weak_raw.fused_rms_mps:.3f}`, `{weak_adaptive.fused_rms_mps:.3f}` m/s oldu. Adaptif yol bu sentetik koşulda gürültü/gecikme uzlaşmasını sağladı; gerçek eşikler kart verisi ister.",
|
||||
f"- Tek uydu burst'ünde raw WLS RMS `{outlier_raw.wls_rms_mps:.3f}` m/s, robust leave-one-out ile `{outlier_robust.wls_rms_mps:.3f}` m/s. ESKF son RMS farkı daha küçük; çünkü mevcut residual covariance inflation aggregate hatayı zaten zayıflatıyor. Robust kapı yine de hatalı kanalı bütün epoch'u reddetmeden ayırır.",
|
||||
f"- Common clock-drift step raw velocity RMS'ini bozmadı; clock-drift state RMS'i `{clock_raw.clock_drift_rms_mps:.3f}` m/s. Dördüncü WLS bilinmeyeni ortak frekans hatasını üç hız bileşeninden ayırıyor.",
|
||||
"",
|
||||
"## Mimari kararı",
|
||||
"",
|
||||
"- Nominal sentetik model ham Doppler'in sabit pre-IIR gecikmesini önlediğini gösterir; fakat Genesys2 kart-kalibreli noise regresyonunda tam ham yol fazla gürültülüdür. Production ana yolu: hareket-adaptif Doppler -> bandwidth-aware sigma -> kanal validity/outlier kontrolü -> 5 Hz robust velocity WLS + gerçek covariance -> adapter tabanları -> ESKF.",
|
||||
"- ESKF yalnız 3D aggregate velocity gördüğü için tek uydu hatasını sonradan teşhis edemez. Per-satellite robust gate/FDE WLS öncesinde kalmalıdır.",
|
||||
"- Ayrı sabit `alpha=0.05` yol PPS drift predictor ve standalone sakin-hız kalite metriği için tutulur; ham snapshot tanısal/offline analiz için korunur.",
|
||||
"- Production adaptif IIR sakin/hareketli/yüksek-ivme için `alpha=0.05/0.20/0.40` seçer. Alpha büyüdükçe EWMA noise gain'i WLS sigma'sına yansıtılır; böylece FDE ve ESKF covariance bant genişliğini bilir.",
|
||||
"",
|
||||
"## Model sınırları",
|
||||
"",
|
||||
"- Sonuçlar sentetik hata büyüklüklerine koşulludur; kartta yüzde kazanç iddiası değildir.",
|
||||
"- Uydu geometrisi sabit sekiz-kanal local LOS modelidir. Satellite velocity/clock düzeltmelerinin doğru yapıldığı varsayılmıştır.",
|
||||
"- ESKF, bu soruya etki eden `[velocity, accel-bias]` alt sistemine indirgenmiştir; attitude, lever-arm ve tam position cross-covariance modellenmemiştir.",
|
||||
"- Tam firmware'deki position update'in velocity/accel-bias cross-covariance üzerinden sağlayabileceği kısmi recovery bu azaltılmış modelde yoktur. Bu nedenle uzun NIS-ret zincirinin mutlak büyüklüğü değil, pre-IIR'in gecikme ve ret mekanizması karar girdisidir.",
|
||||
"- Carrier loop gürültüsü C/N0 sigma yasasıyla üretilmiştir. Gerçek correlation, oscillator phase noise ve cycle slip dağılımları kart loguyla kalibre edilmelidir.",
|
||||
"- Robust WLS'nin `3.5 sigma`, en az altı uydu ve bütün tek-kanal leave-one-out seçimi C implementasyonuyla aynıdır. RTL değişmez; raw/filtered ayrımı firmware'dedir.",
|
||||
"",
|
||||
]
|
||||
)
|
||||
path.write_text("\n".join(lines), encoding="utf-8")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--seeds", type=int, default=20)
|
||||
parser.add_argument("--jobs", type=int, default=4)
|
||||
parser.add_argument(
|
||||
"--output",
|
||||
type=Path,
|
||||
default=Path(__file__).resolve().parent / "output",
|
||||
)
|
||||
arguments = parser.parse_args()
|
||||
arguments.output.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
scenarios = flight_scenarios()
|
||||
tasks = [
|
||||
(scenario_index, seed)
|
||||
for scenario_index in range(len(scenarios))
|
||||
for seed in range(1, arguments.seeds + 1)
|
||||
]
|
||||
grouped: dict[tuple[int, DopplerPath], list[TrialMetrics]] = {
|
||||
(scenario_index, doppler_path): []
|
||||
for scenario_index in range(len(scenarios))
|
||||
for doppler_path in PATH_ORDER
|
||||
}
|
||||
with ProcessPoolExecutor(max_workers=max(1, arguments.jobs)) as executor:
|
||||
for scenario_index, seed, metrics in executor.map(_worker, tasks):
|
||||
for item in metrics:
|
||||
grouped[(scenario_index, item.path)].append(item)
|
||||
print(f"completed {scenarios[scenario_index].name} seed={seed}", flush=True)
|
||||
|
||||
summaries = [
|
||||
summarize(grouped[(scenario_index, doppler_path)])
|
||||
for scenario_index in range(len(scenarios))
|
||||
for doppler_path in PATH_ORDER
|
||||
]
|
||||
write_csv(arguments.output / "doppler_eskf_summary.csv", summaries)
|
||||
write_report(
|
||||
arguments.output / "DOPPLER_ESKF_ANALYSIS.md",
|
||||
summaries,
|
||||
arguments.seeds,
|
||||
)
|
||||
_summary_plot(arguments.output / "05_doppler_eskf_velocity_rms.png", summaries)
|
||||
|
||||
airliner = run_doppler_eskf_scenario(
|
||||
next(item for item in scenarios if item.name == "airliner_maneuver"), 1
|
||||
)
|
||||
_maneuver_trace_plot(arguments.output / "06_airliner_maneuver_trace.png", airliner)
|
||||
outlier = run_doppler_eskf_scenario(
|
||||
next(item for item in scenarios if item.name == "single_satellite_outliers"), 1
|
||||
)
|
||||
_outlier_plot(arguments.output / "07_outlier_wls_nis.png", outlier)
|
||||
weak = run_doppler_eskf_scenario(
|
||||
next(item for item in scenarios if item.name == "low_cn0_airliner"), 1
|
||||
)
|
||||
_filtering_plot(arguments.output / "08_low_cn0_wls_vs_eskf.png", weak)
|
||||
print(f"wrote Doppler/ESKF analysis to {arguments.output}", flush=True)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,425 @@
|
||||
"""Run the complete DLL-quality/WLS and code-smoothing comparison."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import math
|
||||
from concurrent.futures import ProcessPoolExecutor
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from .code_smoothing_model import run_smoothing_trial
|
||||
from .firmware_wls_model import SigmaMethod
|
||||
from .measurement_scenarios import EpochSample, run_scenario, scenario_matrix
|
||||
|
||||
|
||||
METHOD_ORDER = (
|
||||
SigmaMethod.CURRENT,
|
||||
SigmaMethod.NAIVE_ADD_DLL,
|
||||
SigmaMethod.DLL_EXCESS,
|
||||
SigmaMethod.DLL_EXCESS_GUARD,
|
||||
SigmaMethod.ORACLE,
|
||||
)
|
||||
METHOD_LABEL = {
|
||||
SigmaMethod.CURRENT: "Mevcut C/N0",
|
||||
SigmaMethod.NAIVE_ADD_DLL: "Naif DLL ekle",
|
||||
SigmaMethod.DLL_EXCESS: "DLL excess",
|
||||
SigmaMethod.DLL_EXCESS_GUARD: "DLL excess+mean",
|
||||
SigmaMethod.ORACLE: "Oracle variance",
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Summary:
|
||||
scenario: str
|
||||
method: SigmaMethod
|
||||
epochs: int
|
||||
horizontal_rms_m: float
|
||||
position_3d_rms_m: float
|
||||
position_3d_p95_m: float
|
||||
valid_percent: float
|
||||
valid_3d_rms_m: float
|
||||
horizontal_2sigma_coverage_percent: float
|
||||
false_valid_over_25m_percent: float
|
||||
fde_percent: float
|
||||
mean_measurement_sigma_m: float
|
||||
|
||||
|
||||
def _worker(task: tuple[int, int]) -> tuple[int, int, list[EpochSample]]:
|
||||
scenario_index, seed = task
|
||||
scenario = scenario_matrix()[scenario_index]
|
||||
return scenario_index, seed, run_scenario(scenario, seed, METHOD_ORDER)
|
||||
|
||||
|
||||
def _rms(values: np.ndarray) -> float:
|
||||
return float(np.sqrt(np.mean(np.square(values)))) if len(values) else math.nan
|
||||
|
||||
|
||||
def summarize(scenario_name: str, method: SigmaMethod, samples: list[EpochSample]) -> Summary:
|
||||
selected = [
|
||||
item
|
||||
for item in samples
|
||||
if item.method == method and item.time_s >= 5.0
|
||||
]
|
||||
horizontal = np.asarray([item.horizontal_error_m for item in selected])
|
||||
error_3d = np.asarray([item.position_error_3d_m for item in selected])
|
||||
valid = np.asarray([item.valid for item in selected], dtype=bool)
|
||||
horizontal_sigma = np.asarray([item.horizontal_sigma_m for item in selected])
|
||||
fde = np.asarray([item.fde_applied for item in selected], dtype=bool)
|
||||
mean_sigma = np.asarray([item.mean_measurement_sigma_m for item in selected])
|
||||
false_valid = valid & (error_3d > 25.0)
|
||||
return Summary(
|
||||
scenario=scenario_name,
|
||||
method=method,
|
||||
epochs=len(selected),
|
||||
horizontal_rms_m=_rms(horizontal),
|
||||
position_3d_rms_m=_rms(error_3d),
|
||||
position_3d_p95_m=float(np.percentile(error_3d, 95.0)),
|
||||
valid_percent=100.0 * float(np.mean(valid)),
|
||||
valid_3d_rms_m=_rms(error_3d[valid]),
|
||||
horizontal_2sigma_coverage_percent=100.0
|
||||
* float(np.mean(horizontal <= 2.0 * horizontal_sigma)),
|
||||
false_valid_over_25m_percent=100.0 * float(np.mean(false_valid)),
|
||||
fde_percent=100.0 * float(np.mean(fde)),
|
||||
mean_measurement_sigma_m=float(np.mean(mean_sigma)),
|
||||
)
|
||||
|
||||
|
||||
def write_csv(path: Path, summaries: list[Summary]) -> None:
|
||||
with path.open("w", newline="", encoding="utf-8") as output:
|
||||
writer = csv.DictWriter(output, fieldnames=list(Summary.__dataclass_fields__))
|
||||
writer.writeheader()
|
||||
for item in summaries:
|
||||
row = dict(item.__dict__)
|
||||
row["method"] = item.method.value
|
||||
writer.writerow(row)
|
||||
|
||||
|
||||
def _comparison_plot(
|
||||
path: Path,
|
||||
summaries: list[Summary],
|
||||
field: str,
|
||||
ylabel: str,
|
||||
*,
|
||||
logarithmic: bool = False,
|
||||
) -> None:
|
||||
scenarios = scenario_matrix()
|
||||
x = np.arange(len(scenarios))
|
||||
width = 0.16
|
||||
figure, axis = plt.subplots(figsize=(15, 6.5))
|
||||
for method_index, method in enumerate(METHOD_ORDER):
|
||||
values = [
|
||||
getattr(
|
||||
next(
|
||||
item
|
||||
for item in summaries
|
||||
if item.scenario == scenario.name and item.method == method
|
||||
),
|
||||
field,
|
||||
)
|
||||
for scenario in scenarios
|
||||
]
|
||||
axis.bar(
|
||||
x + (method_index - 2) * width,
|
||||
values,
|
||||
width,
|
||||
label=METHOD_LABEL[method],
|
||||
)
|
||||
axis.set_ylabel(ylabel)
|
||||
if logarithmic:
|
||||
axis.set_yscale("log")
|
||||
axis.set_xticks(x)
|
||||
axis.set_xticklabels([item.name for item in scenarios], rotation=28, ha="right")
|
||||
axis.grid(axis="y", alpha=0.25)
|
||||
axis.legend(ncol=3)
|
||||
figure.tight_layout()
|
||||
figure.savefig(path, dpi=150)
|
||||
plt.close(figure)
|
||||
|
||||
|
||||
def _transition_plot(path: Path, samples: list[EpochSample]) -> None:
|
||||
figure, axes = plt.subplots(2, 1, figsize=(12, 7), sharex=True)
|
||||
for method in (
|
||||
SigmaMethod.CURRENT,
|
||||
SigmaMethod.DLL_EXCESS,
|
||||
SigmaMethod.DLL_EXCESS_GUARD,
|
||||
):
|
||||
selected = [item for item in samples if item.method == method]
|
||||
axes[0].plot(
|
||||
[item.time_s for item in selected],
|
||||
[item.position_error_3d_m for item in selected],
|
||||
label=METHOD_LABEL[method],
|
||||
linewidth=1.2,
|
||||
)
|
||||
reference = [item for item in samples if item.method == SigmaMethod.CURRENT]
|
||||
axes[1].plot(
|
||||
[item.time_s for item in reference],
|
||||
[item.maximum_dll_ratio for item in reference],
|
||||
color="tab:red",
|
||||
)
|
||||
axes[0].axvspan(22.0, 45.0, color="tab:orange", alpha=0.15)
|
||||
axes[1].axvspan(22.0, 45.0, color="tab:orange", alpha=0.15)
|
||||
axes[0].set_ylabel("3D konum hatası (m)")
|
||||
axes[1].set_ylabel("Maks. DLL variance ratio")
|
||||
axes[1].set_xlabel("Zaman (s)")
|
||||
axes[0].legend()
|
||||
for axis in axes:
|
||||
axis.grid(alpha=0.25)
|
||||
figure.tight_layout()
|
||||
figure.savefig(path, dpi=150)
|
||||
plt.close(figure)
|
||||
|
||||
|
||||
def smoothing_summary(seed_count: int) -> dict[str, dict[str, float]]:
|
||||
result: dict[str, dict[str, float]] = {}
|
||||
for label, cfo_mps in (
|
||||
("cfo_compensated", 0.0),
|
||||
("cfo_residual_0p5_mps", 0.5),
|
||||
("cfo_residual_3_mps", 3.0),
|
||||
):
|
||||
trials = [
|
||||
run_smoothing_trial(seed, doppler_cfo_mps=cfo_mps)
|
||||
for seed in range(1, seed_count + 1)
|
||||
]
|
||||
result[label] = {
|
||||
field: float(np.mean([getattr(item, field) for item in trials]))
|
||||
for field in trials[0].__dataclass_fields__
|
||||
}
|
||||
return result
|
||||
|
||||
|
||||
def _smoothing_plot(path: Path, smoothing: dict[str, dict[str, float]]) -> None:
|
||||
labels = list(smoothing)
|
||||
x = np.arange(len(labels))
|
||||
width = 0.20
|
||||
figure, axis = plt.subplots(figsize=(10, 5.5))
|
||||
for offset, (field, title) in enumerate(
|
||||
(
|
||||
("raw_rmse_m", "Ham code"),
|
||||
("scalar_iir_rmse_m", "Kör IIR"),
|
||||
("doppler_smoothed_rmse_m", "Raw Doppler-predicted"),
|
||||
(
|
||||
"firmware_iir_doppler_smoothed_rmse_m",
|
||||
"FW-IIR Doppler-predicted",
|
||||
),
|
||||
)
|
||||
):
|
||||
axis.bar(
|
||||
x + (offset - 1.5) * width,
|
||||
[smoothing[label][field] for label in labels],
|
||||
width,
|
||||
label=title,
|
||||
)
|
||||
axis.set_xticks(x)
|
||||
axis.set_xticklabels(labels)
|
||||
axis.set_ylabel("Range RMSE (m)")
|
||||
axis.set_yscale("log")
|
||||
axis.grid(axis="y", alpha=0.25)
|
||||
axis.legend()
|
||||
figure.tight_layout()
|
||||
figure.savefig(path, dpi=150)
|
||||
plt.close(figure)
|
||||
|
||||
|
||||
def write_report(
|
||||
path: Path,
|
||||
summaries: list[Summary],
|
||||
focused_rms: dict[str, dict[SigmaMethod, float]],
|
||||
smoothing: dict[str, dict[str, float]],
|
||||
seed_count: int,
|
||||
) -> None:
|
||||
current = {
|
||||
item.scenario: item
|
||||
for item in summaries
|
||||
if item.method == SigmaMethod.CURRENT
|
||||
}
|
||||
lines = [
|
||||
"# GPS code measurement model sonuçları",
|
||||
"",
|
||||
f"Monte Carlo seed sayısı: `{seed_count}`. İlk 5 saniye metriklere alınmadı.",
|
||||
"",
|
||||
"## Yöntem karşılaştırması",
|
||||
"",
|
||||
"| Senaryo | Yöntem | 3D RMS (m) | Mevcuda göre | P95 (m) | Valid (%) | >25 m false-valid (%) |",
|
||||
"|---|---|---:|---:|---:|---:|---:|",
|
||||
]
|
||||
for scenario in scenario_matrix():
|
||||
baseline = current[scenario.name].position_3d_rms_m
|
||||
for method in METHOD_ORDER:
|
||||
item = next(
|
||||
value
|
||||
for value in summaries
|
||||
if value.scenario == scenario.name and value.method == method
|
||||
)
|
||||
improvement = 100.0 * (baseline - item.position_3d_rms_m) / baseline
|
||||
lines.append(
|
||||
f"| {scenario.name} | {METHOD_LABEL[method]} | "
|
||||
f"{item.position_3d_rms_m:.2f} | {improvement:+.1f}% | "
|
||||
f"{item.position_3d_p95_m:.2f} | {item.valid_percent:.1f} | "
|
||||
f"{item.false_valid_over_25m_percent:.2f} |"
|
||||
)
|
||||
|
||||
lines.extend(
|
||||
[
|
||||
"",
|
||||
"## Modelden çıkan karar özeti",
|
||||
"",
|
||||
"- C/N0 ile uyumlu nominal DLL jitter'da `dll_excess`, mevcut çözümü değiştirmemelidir.",
|
||||
"- Bütün measurement sigmaları aynı sabitle çarpılırsa WLS konum noktası değişmez; yalnız covariance/integrity değişir. Fakat ortak DLL jitter çarpanı, C/N0/elevation bileşimi kanaldan kanala farklı olduğu için toplam sigmaları eşit oranda büyütmez.",
|
||||
"- Kanal-bağımlı ve C/N0'da görünmeyen excess jitter varsa dinamik sigma o kanalı down-weight ederek konumu iyileştirebilir.",
|
||||
"- Stable multipath bias düşük DLL variance bırakabildiği için variance-only yöntem çözüm değildir.",
|
||||
"- 50 Hz'de DLL ratio alpha=0.10 yaklaşık 0.19 s zaman sabiti ve 0.57 s yüzde-95 geçiş verir.",
|
||||
"- Aşağıdaki yüzdeler seçilen sentetik hata büyüklüklerine koşulludur; kart için kazanç tahmini değildir.",
|
||||
"",
|
||||
"## Bozulma penceresi odaklı 3D RMS",
|
||||
"",
|
||||
"| Senaryo/pencere | Mevcut (m) | DLL excess (m) | DLL excess+mean (m) | Oracle variance (m) |",
|
||||
"|---|---:|---:|---:|---:|",
|
||||
]
|
||||
)
|
||||
for label, values in focused_rms.items():
|
||||
lines.append(
|
||||
f"| {label} | {values[SigmaMethod.CURRENT]:.2f} | "
|
||||
f"{values[SigmaMethod.DLL_EXCESS]:.2f} | "
|
||||
f"{values[SigmaMethod.DLL_EXCESS_GUARD]:.2f} | "
|
||||
f"{values[SigmaMethod.ORACLE]:.2f} |"
|
||||
)
|
||||
lines.extend(
|
||||
[
|
||||
"",
|
||||
"## Code smoothing ve carrier CFO",
|
||||
"",
|
||||
"| Carrier-rate koşulu | Ham RMSE (m) | Kör IIR RMSE (m) | Raw Doppler RMSE (m) | FW-IIR Doppler RMSE (m) | Kör IIR peak (m) | Raw Doppler peak (m) | FW-IIR Doppler peak (m) |",
|
||||
"|---|---:|---:|---:|---:|---:|---:|---:|",
|
||||
]
|
||||
)
|
||||
for label, values in smoothing.items():
|
||||
lines.append(
|
||||
f"| {label} | {values['raw_rmse_m']:.2f} | "
|
||||
f"{values['scalar_iir_rmse_m']:.2f} | "
|
||||
f"{values['doppler_smoothed_rmse_m']:.2f} | "
|
||||
f"{values['firmware_iir_doppler_smoothed_rmse_m']:.2f} | "
|
||||
f"{values['scalar_iir_peak_dynamic_error_m']:.2f} | "
|
||||
f"{values['doppler_peak_dynamic_error_m']:.2f} | "
|
||||
f"{values['firmware_iir_doppler_peak_dynamic_error_m']:.2f} |"
|
||||
)
|
||||
|
||||
lines.extend(
|
||||
[
|
||||
"",
|
||||
"## Yorum sınırları",
|
||||
"",
|
||||
"- `current_cn0`, firmware'deki URA/elevation/C/N0 sigma modelidir.",
|
||||
"- `naive_add_dll`, aynı thermal jitter'ı iki kere saymanın negatif kontrolüdür.",
|
||||
"- `dll_excess`, yalnız ölçülen DLL variance'ın C/N0 beklentisini aşan kısmını code variance'a ekler.",
|
||||
"- `dll_excess_guard`, variance'a ek olarak sürekli discriminator mean stress'ini kullanır.",
|
||||
"- `oracle_variance`, sentetik gerçek jitter çarpanını bilir; yalnız variance kaynakları için erişilebilir üst sınırdır, bias'ı bilmez.",
|
||||
"- Stable multipath DLL'nin yeni denge noktasında düşük residual variance bırakabilir. Bu nedenle variance-only yönteminin onu çözmesi beklenmez.",
|
||||
"- Discriminator gürültü ölçeği kart correlator kayıtları gelene kadar kalibrasyon varsayımıdır; oran tabanlı yöntem mutlak ölçeğe göre daha az hassastır.",
|
||||
"- Sonuçlar bitstream veya firmware değişikliği değildir; karar öncesi executable tasarım keşfidir.",
|
||||
"",
|
||||
]
|
||||
)
|
||||
path.write_text("\n".join(lines), encoding="utf-8")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--seeds", type=int, default=12)
|
||||
parser.add_argument("--jobs", type=int, default=4)
|
||||
parser.add_argument(
|
||||
"--output",
|
||||
type=Path,
|
||||
default=Path(__file__).resolve().parent / "output",
|
||||
)
|
||||
arguments = parser.parse_args()
|
||||
arguments.output.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
scenarios = scenario_matrix()
|
||||
tasks = [
|
||||
(scenario_index, seed)
|
||||
for scenario_index in range(len(scenarios))
|
||||
for seed in range(1, arguments.seeds + 1)
|
||||
]
|
||||
grouped: dict[int, list[EpochSample]] = {
|
||||
index: [] for index in range(len(scenarios))
|
||||
}
|
||||
transition_samples: list[EpochSample] = []
|
||||
with ProcessPoolExecutor(max_workers=max(1, arguments.jobs)) as executor:
|
||||
for scenario_index, seed, samples in executor.map(_worker, tasks):
|
||||
grouped[scenario_index].extend(samples)
|
||||
if scenarios[scenario_index].name == "noise_burst_recovery" and seed == 1:
|
||||
transition_samples = samples
|
||||
print(
|
||||
f"completed {scenarios[scenario_index].name} seed={seed}",
|
||||
flush=True,
|
||||
)
|
||||
|
||||
summaries = [
|
||||
summarize(scenario.name, method, grouped[index])
|
||||
for index, scenario in enumerate(scenarios)
|
||||
for method in METHOD_ORDER
|
||||
]
|
||||
focused_windows = {
|
||||
"noise_burst_recovery [22,45) s": ("noise_burst_recovery", 22.0, 45.0),
|
||||
"dll_transient_stress [25,34) s": ("dll_transient_stress", 25.0, 34.0),
|
||||
"cn0_step_recovery [5,30) s": ("cn0_step_recovery", 5.0, 30.0),
|
||||
}
|
||||
focused_rms: dict[str, dict[SigmaMethod, float]] = {}
|
||||
for label, (scenario_name, start_s, stop_s) in focused_windows.items():
|
||||
scenario_index = next(
|
||||
index
|
||||
for index, scenario in enumerate(scenarios)
|
||||
if scenario.name == scenario_name
|
||||
)
|
||||
focused_rms[label] = {}
|
||||
for method in METHOD_ORDER:
|
||||
values = np.asarray(
|
||||
[
|
||||
item.position_error_3d_m
|
||||
for item in grouped[scenario_index]
|
||||
if item.method == method
|
||||
and start_s <= item.time_s < stop_s
|
||||
]
|
||||
)
|
||||
focused_rms[label][method] = _rms(values)
|
||||
smoothing = smoothing_summary(max(arguments.seeds, 20))
|
||||
|
||||
write_csv(arguments.output / "scenario_summary.csv", summaries)
|
||||
write_report(
|
||||
arguments.output / "ANALYSIS.md",
|
||||
summaries,
|
||||
focused_rms,
|
||||
smoothing,
|
||||
arguments.seeds,
|
||||
)
|
||||
_comparison_plot(
|
||||
arguments.output / "01_position_3d_rms.png",
|
||||
summaries,
|
||||
"position_3d_rms_m",
|
||||
"3D RMS (m)",
|
||||
logarithmic=True,
|
||||
)
|
||||
_comparison_plot(
|
||||
arguments.output / "02_solution_availability.png",
|
||||
summaries,
|
||||
"valid_percent",
|
||||
"Integrity-valid çözüm (%)",
|
||||
)
|
||||
_transition_plot(
|
||||
arguments.output / "03_noise_burst_transition.png",
|
||||
transition_samples,
|
||||
)
|
||||
_smoothing_plot(arguments.output / "04_code_smoothing_cfo.png", smoothing)
|
||||
print(f"wrote analysis to {arguments.output}", flush=True)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,132 @@
|
||||
"""One-dimensional code smoothing study with carrier/Doppler aiding."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SmoothingResult:
|
||||
raw_rmse_m: float
|
||||
scalar_iir_rmse_m: float
|
||||
doppler_smoothed_rmse_m: float
|
||||
firmware_iir_doppler_smoothed_rmse_m: float
|
||||
scalar_iir_peak_dynamic_error_m: float
|
||||
doppler_peak_dynamic_error_m: float
|
||||
firmware_iir_doppler_peak_dynamic_error_m: float
|
||||
|
||||
|
||||
def run_smoothing_trial(
|
||||
seed: int,
|
||||
*,
|
||||
duration_s: float = 80.0,
|
||||
tracking_dt_s: float = 0.020,
|
||||
wls_dt_s: float = 0.200,
|
||||
code_sigma_m: float = 4.0,
|
||||
doppler_sigma_mps: float = 0.25,
|
||||
doppler_cfo_mps: float = 0.0,
|
||||
alpha: float = 0.10,
|
||||
firmware_doppler_alpha: float = 0.05,
|
||||
) -> SmoothingResult:
|
||||
"""Compare raw code, blind IIR and Doppler-propagated code smoothing.
|
||||
|
||||
The truth includes a range-rate step so a visually smooth but lagging
|
||||
scalar IIR is penalized. ``doppler_cfo_mps`` represents residual receiver
|
||||
carrier frequency offset after the nominal IF subtraction.
|
||||
"""
|
||||
|
||||
if abs(round(wls_dt_s / tracking_dt_s) - wls_dt_s / tracking_dt_s) > 1.0e-12:
|
||||
raise ValueError("WLS period must be an integer number of tracking periods")
|
||||
rng = np.random.default_rng(seed)
|
||||
tracking_time_s = np.arange(0.0, duration_s, tracking_dt_s)
|
||||
true_rate_tracking = np.where(
|
||||
tracking_time_s < 20.0,
|
||||
-120.0,
|
||||
np.where(tracking_time_s < 45.0, 230.0, -40.0),
|
||||
)
|
||||
true_range_tracking = np.empty_like(tracking_time_s)
|
||||
true_range_tracking[0] = 21_000_000.0
|
||||
true_range_tracking[1:] = (
|
||||
true_range_tracking[0]
|
||||
+ np.cumsum(true_rate_tracking[:-1] * tracking_dt_s)
|
||||
)
|
||||
measured_rate_tracking = (
|
||||
true_rate_tracking
|
||||
+ doppler_cfo_mps
|
||||
+ rng.normal(0.0, doppler_sigma_mps, len(tracking_time_s))
|
||||
)
|
||||
filtered_rate_tracking = np.empty_like(measured_rate_tracking)
|
||||
filtered_rate_tracking[0] = measured_rate_tracking[0]
|
||||
for index in range(1, len(filtered_rate_tracking)):
|
||||
filtered_rate_tracking[index] = (
|
||||
firmware_doppler_alpha * measured_rate_tracking[index]
|
||||
+ (1.0 - firmware_doppler_alpha) * filtered_rate_tracking[index - 1]
|
||||
)
|
||||
|
||||
tracking_per_wls = int(round(wls_dt_s / tracking_dt_s))
|
||||
wls_indices = np.arange(0, len(tracking_time_s), tracking_per_wls)
|
||||
time_s = tracking_time_s[wls_indices]
|
||||
true_range = true_range_tracking[wls_indices]
|
||||
raw_code = true_range + rng.normal(0.0, code_sigma_m, len(time_s))
|
||||
|
||||
scalar = np.empty_like(raw_code)
|
||||
doppler_smoothed = np.empty_like(raw_code)
|
||||
firmware_iir_doppler_smoothed = np.empty_like(raw_code)
|
||||
scalar[0] = raw_code[0]
|
||||
doppler_smoothed[0] = raw_code[0]
|
||||
firmware_iir_doppler_smoothed[0] = raw_code[0]
|
||||
for index in range(1, len(time_s)):
|
||||
scalar[index] = alpha * raw_code[index] + (1.0 - alpha) * scalar[index - 1]
|
||||
start = wls_indices[index - 1]
|
||||
stop = wls_indices[index]
|
||||
raw_range_delta = (
|
||||
float(np.sum(measured_rate_tracking[start:stop]))
|
||||
* tracking_dt_s
|
||||
)
|
||||
filtered_range_delta = (
|
||||
float(np.sum(filtered_rate_tracking[start:stop]))
|
||||
* tracking_dt_s
|
||||
)
|
||||
prediction = doppler_smoothed[index - 1] + raw_range_delta
|
||||
doppler_smoothed[index] = (
|
||||
prediction + alpha * (raw_code[index] - prediction)
|
||||
)
|
||||
filtered_prediction = (
|
||||
firmware_iir_doppler_smoothed[index - 1]
|
||||
+ filtered_range_delta
|
||||
)
|
||||
firmware_iir_doppler_smoothed[index] = (
|
||||
filtered_prediction
|
||||
+ alpha * (raw_code[index] - filtered_prediction)
|
||||
)
|
||||
|
||||
warm = time_s >= 5.0
|
||||
dynamic = ((time_s >= 20.0) & (time_s < 25.0)) | (
|
||||
(time_s >= 45.0) & (time_s < 50.0)
|
||||
)
|
||||
rmse = lambda error, mask: float(np.sqrt(np.mean(np.square(error[mask]))))
|
||||
return SmoothingResult(
|
||||
raw_rmse_m=rmse(raw_code - true_range, warm),
|
||||
scalar_iir_rmse_m=rmse(scalar - true_range, warm),
|
||||
doppler_smoothed_rmse_m=rmse(doppler_smoothed - true_range, warm),
|
||||
firmware_iir_doppler_smoothed_rmse_m=rmse(
|
||||
firmware_iir_doppler_smoothed - true_range,
|
||||
warm,
|
||||
),
|
||||
scalar_iir_peak_dynamic_error_m=float(
|
||||
np.max(np.abs(scalar[dynamic] - true_range[dynamic]))
|
||||
),
|
||||
doppler_peak_dynamic_error_m=float(
|
||||
np.max(np.abs(doppler_smoothed[dynamic] - true_range[dynamic]))
|
||||
),
|
||||
firmware_iir_doppler_peak_dynamic_error_m=float(
|
||||
np.max(
|
||||
np.abs(
|
||||
firmware_iir_doppler_smoothed[dynamic]
|
||||
- true_range[dynamic]
|
||||
)
|
||||
)
|
||||
),
|
||||
)
|
||||
@@ -0,0 +1,657 @@
|
||||
"""Project-specific Doppler WLS to ESKF timing/noise model.
|
||||
|
||||
The model compares four paths using one shared 50 Hz tracking stream and one
|
||||
shared 100 Hz IMU stream:
|
||||
|
||||
* the legacy fixed-alpha Doppler IIR before 5 Hz velocity WLS;
|
||||
* raw Doppler into WLS, as a nominal-model comparison rather than production;
|
||||
* a global IMU-driven adaptive-alpha IIR;
|
||||
* raw Doppler with the per-satellite robust WLS gate used before the ESKF.
|
||||
|
||||
The ESKF is reduced to the velocity and accelerometer-bias error states that
|
||||
matter to this experiment. Its process noise, 3-D measurement covariance and
|
||||
99.9-percent velocity NIS gate mirror the project firmware. Attitude and
|
||||
position are intentionally outside this model's scope.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum
|
||||
from typing import Callable
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
IMU_HZ = 100.0
|
||||
TRACKING_HZ = 50.0
|
||||
VELOCITY_WLS_HZ = 5.0
|
||||
IMU_DT_S = 1.0 / IMU_HZ
|
||||
TRACKING_STEPS = int(round(IMU_HZ / TRACKING_HZ))
|
||||
WLS_STEPS = int(round(IMU_HZ / VELOCITY_WLS_HZ))
|
||||
FIXED_DOPPLER_ALPHA = 0.05
|
||||
CN0_ALPHA = 0.20
|
||||
VELOCITY_NIS_GATE = 16.26623619623813
|
||||
PROCESS_ACCEL_NOISE_DENSITY = 0.025
|
||||
ACCEL_BIAS_RANDOM_WALK = 0.0015
|
||||
MINIMUM_HORIZONTAL_VELOCITY_STD_MPS = 2.0
|
||||
MINIMUM_VERTICAL_VELOCITY_STD_MPS = 3.0
|
||||
|
||||
|
||||
class DopplerPath(str, Enum):
|
||||
FIXED_IIR = "fixed_iir_wls_eskf"
|
||||
RAW = "raw_wls_eskf"
|
||||
ADAPTIVE_IIR = "adaptive_iir_wls_eskf"
|
||||
RAW_ROBUST = "raw_robust_wls_eskf"
|
||||
|
||||
|
||||
PATH_ORDER = (
|
||||
DopplerPath.FIXED_IIR,
|
||||
DopplerPath.RAW,
|
||||
DopplerPath.ADAPTIVE_IIR,
|
||||
DopplerPath.RAW_ROBUST,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FlightScenario:
|
||||
name: str
|
||||
description: str
|
||||
duration_s: float
|
||||
initial_velocity_mps: np.ndarray
|
||||
acceleration: Callable[[float], np.ndarray]
|
||||
cn0_offset: Callable[[float], float]
|
||||
noise_multiplier: Callable[[float, int], float]
|
||||
outlier_mps: Callable[[float, int], float]
|
||||
receiver_clock_drift_mps: Callable[[float], float]
|
||||
gps_gaps: tuple[tuple[float, float], ...] = ()
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class VelocityWlsResult:
|
||||
velocity_mps: np.ndarray
|
||||
clock_drift_mps: float
|
||||
covariance_mps2: np.ndarray
|
||||
residual_mps: np.ndarray
|
||||
residual_rms_mps: float
|
||||
covariance_scale: float
|
||||
excluded_satellite: int | None
|
||||
|
||||
|
||||
@dataclass
|
||||
class AdaptiveAlpha:
|
||||
alpha: float = FIXED_DOPPLER_ALPHA
|
||||
hold_samples: int = 0
|
||||
filtered_acceleration_mps2: np.ndarray = field(
|
||||
default_factory=lambda: np.zeros(3, dtype=float)
|
||||
)
|
||||
|
||||
def update(
|
||||
self,
|
||||
measured_acceleration_mps2: np.ndarray,
|
||||
median_cn0_dbhz: float,
|
||||
) -> float:
|
||||
# A raw 100 Hz accelerometer sample is too noisy for coefficient
|
||||
# switching. This 50 Hz vector EWMA has an approximately 0.16 s time
|
||||
# constant and, unlike smoothing |a|, does not retain the positive
|
||||
# magnitude bias of zero-mean sensor noise.
|
||||
motion_alpha = 0.12
|
||||
self.filtered_acceleration_mps2 += motion_alpha * (
|
||||
measured_acceleration_mps2 - self.filtered_acceleration_mps2
|
||||
)
|
||||
magnitude = float(np.linalg.norm(self.filtered_acceleration_mps2))
|
||||
|
||||
if median_cn0_dbhz >= 46.0:
|
||||
quiet_alpha = 0.08
|
||||
elif median_cn0_dbhz >= 37.0:
|
||||
quiet_alpha = FIXED_DOPPLER_ALPHA
|
||||
else:
|
||||
quiet_alpha = 0.03
|
||||
|
||||
if magnitude >= 2.5:
|
||||
self.alpha = 0.40
|
||||
self.hold_samples = 25
|
||||
elif magnitude >= 0.55:
|
||||
self.alpha = 0.20
|
||||
self.hold_samples = 25
|
||||
elif self.hold_samples > 0:
|
||||
self.hold_samples -= 1
|
||||
else:
|
||||
self.alpha = quiet_alpha
|
||||
return self.alpha
|
||||
|
||||
|
||||
@dataclass
|
||||
class VelocityBiasEskf:
|
||||
velocity_mps: np.ndarray
|
||||
accel_bias_mps2: np.ndarray
|
||||
covariance: np.ndarray
|
||||
accepted: int = 0
|
||||
rejected_nis: int = 0
|
||||
last_nis: float = float("nan")
|
||||
|
||||
@classmethod
|
||||
def initialize(
|
||||
cls,
|
||||
initial_velocity_mps: np.ndarray,
|
||||
initial_velocity_error_mps: np.ndarray,
|
||||
) -> "VelocityBiasEskf":
|
||||
covariance = np.zeros((6, 6), dtype=float)
|
||||
covariance[:3, :3] = np.eye(3)
|
||||
covariance[3:, 3:] = np.eye(3) * 0.1**2
|
||||
return cls(
|
||||
velocity_mps=initial_velocity_mps + initial_velocity_error_mps,
|
||||
accel_bias_mps2=np.zeros(3),
|
||||
covariance=covariance,
|
||||
)
|
||||
|
||||
def propagate(self, measured_acceleration_mps2: np.ndarray, dt_s: float) -> None:
|
||||
self.velocity_mps += (
|
||||
measured_acceleration_mps2 - self.accel_bias_mps2
|
||||
) * dt_s
|
||||
|
||||
transition = np.eye(6)
|
||||
transition[:3, 3:] = -np.eye(3) * dt_s
|
||||
accel_variance = PROCESS_ACCEL_NOISE_DENSITY**2
|
||||
bias_variance = ACCEL_BIAS_RANDOM_WALK**2
|
||||
process = np.zeros((6, 6), dtype=float)
|
||||
# Match eskf.c's first-order discrete injection: velocity and bias
|
||||
# driving noise are added after F P F^T, without exact-discretization
|
||||
# dt^2/dt^3 cross terms.
|
||||
process[:3, :3] = np.eye(3) * accel_variance * dt_s
|
||||
process[3:, 3:] = np.eye(3) * bias_variance * dt_s
|
||||
self.covariance = (
|
||||
transition @ self.covariance @ transition.T + process
|
||||
)
|
||||
self.covariance = 0.5 * (self.covariance + self.covariance.T)
|
||||
|
||||
def update_velocity(
|
||||
self,
|
||||
measurement_mps: np.ndarray,
|
||||
covariance_mps2: np.ndarray,
|
||||
) -> bool:
|
||||
innovation = measurement_mps - self.velocity_mps
|
||||
innovation_covariance = self.covariance[:3, :3] + covariance_mps2
|
||||
inverse = np.linalg.inv(innovation_covariance)
|
||||
nis = float(innovation @ inverse @ innovation)
|
||||
self.last_nis = nis
|
||||
if not np.isfinite(nis) or nis > VELOCITY_NIS_GATE:
|
||||
self.rejected_nis += 1
|
||||
return False
|
||||
|
||||
measurement_matrix = np.zeros((3, 6), dtype=float)
|
||||
measurement_matrix[:, :3] = np.eye(3)
|
||||
gain = self.covariance @ measurement_matrix.T @ inverse
|
||||
correction = gain @ innovation
|
||||
self.velocity_mps += correction[:3]
|
||||
self.accel_bias_mps2 += correction[3:]
|
||||
identity = np.eye(6)
|
||||
residual_transform = identity - gain @ measurement_matrix
|
||||
self.covariance = (
|
||||
residual_transform
|
||||
@ self.covariance
|
||||
@ residual_transform.T
|
||||
+ gain @ covariance_mps2 @ gain.T
|
||||
)
|
||||
self.covariance = 0.5 * (self.covariance + self.covariance.T)
|
||||
self.accepted += 1
|
||||
return True
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DopplerEskfTrace:
|
||||
scenario: str
|
||||
path: DopplerPath
|
||||
time_s: np.ndarray
|
||||
truth_velocity_mps: np.ndarray
|
||||
fused_velocity_mps: np.ndarray
|
||||
fused_velocity_covariance_mps2: np.ndarray
|
||||
acceleration_mps2: np.ndarray
|
||||
wls_time_s: np.ndarray
|
||||
wls_velocity_mps: np.ndarray
|
||||
wls_clock_drift_mps: np.ndarray
|
||||
wls_covariance_mps2: np.ndarray
|
||||
wls_covariance_scale: np.ndarray
|
||||
wls_nis: np.ndarray
|
||||
wls_accepted: np.ndarray
|
||||
adaptive_alpha: np.ndarray
|
||||
excluded_satellites: np.ndarray
|
||||
|
||||
|
||||
def _zero_acceleration(_time_s: float) -> np.ndarray:
|
||||
return np.zeros(3)
|
||||
|
||||
|
||||
def _airliner_acceleration(time_s: float) -> np.ndarray:
|
||||
if 15.0 <= time_s < 25.0:
|
||||
return np.asarray([2.0, 0.0, 0.0])
|
||||
if 38.0 <= time_s < 52.0:
|
||||
return np.asarray([0.0, 1.5, 0.25])
|
||||
if 68.0 <= time_s < 76.0:
|
||||
return np.asarray([-2.5, 0.0, -0.15])
|
||||
return np.zeros(3)
|
||||
|
||||
|
||||
def _aggressive_acceleration(time_s: float) -> np.ndarray:
|
||||
if 10.0 <= time_s < 16.0:
|
||||
return np.asarray([6.0, 0.0, 1.0])
|
||||
if 26.0 <= time_s < 33.0:
|
||||
return np.asarray([0.0, 7.0, 0.0])
|
||||
if 46.0 <= time_s < 52.0:
|
||||
return np.asarray([-5.0, -4.0, -1.0])
|
||||
if 64.0 <= time_s < 76.0:
|
||||
phase = 2.0 * np.pi * (time_s - 64.0) / 3.0
|
||||
return np.asarray([3.0 * np.sin(phase), 4.0 * np.cos(phase), 0.5 * np.sin(phase)])
|
||||
return np.zeros(3)
|
||||
|
||||
|
||||
def _constant_zero(_time_s: float) -> float:
|
||||
return 0.0
|
||||
|
||||
|
||||
def _constant_one(_time_s: float, _satellite: int) -> float:
|
||||
return 1.0
|
||||
|
||||
|
||||
def _no_outlier(_time_s: float, _satellite: int) -> float:
|
||||
return 0.0
|
||||
|
||||
|
||||
def flight_scenarios() -> tuple[FlightScenario, ...]:
|
||||
return (
|
||||
FlightScenario(
|
||||
"steady_cruise",
|
||||
"Constant aircraft velocity with nominal open-sky Doppler noise.",
|
||||
90.0,
|
||||
np.asarray([180.0, 20.0, 0.0]),
|
||||
_zero_acceleration,
|
||||
_constant_zero,
|
||||
_constant_one,
|
||||
_no_outlier,
|
||||
_constant_zero,
|
||||
),
|
||||
FlightScenario(
|
||||
"airliner_maneuver",
|
||||
"Moderate longitudinal, turn and climb-rate accelerations.",
|
||||
90.0,
|
||||
np.asarray([120.0, 0.0, 0.0]),
|
||||
_airliner_acceleration,
|
||||
_constant_zero,
|
||||
_constant_one,
|
||||
_no_outlier,
|
||||
_constant_zero,
|
||||
),
|
||||
FlightScenario(
|
||||
"aggressive_maneuver",
|
||||
"High acceleration and oscillatory manoeuvre segments.",
|
||||
90.0,
|
||||
np.asarray([80.0, 0.0, 0.0]),
|
||||
_aggressive_acceleration,
|
||||
_constant_zero,
|
||||
_constant_one,
|
||||
_no_outlier,
|
||||
_constant_zero,
|
||||
),
|
||||
FlightScenario(
|
||||
"low_cn0_airliner",
|
||||
"Airliner manoeuvre with a 10 dB signal-quality loss.",
|
||||
90.0,
|
||||
np.asarray([120.0, 0.0, 0.0]),
|
||||
_airliner_acceleration,
|
||||
lambda _time_s: -10.0,
|
||||
_constant_one,
|
||||
_no_outlier,
|
||||
_constant_zero,
|
||||
),
|
||||
FlightScenario(
|
||||
"single_satellite_outliers",
|
||||
"One channel has repeatable 12 m/s Doppler bursts.",
|
||||
90.0,
|
||||
np.asarray([120.0, 0.0, 0.0]),
|
||||
_airliner_acceleration,
|
||||
_constant_zero,
|
||||
_constant_one,
|
||||
lambda time_s, satellite: (
|
||||
12.0
|
||||
if satellite == 2
|
||||
and (
|
||||
30.0 <= time_s < 31.0
|
||||
or 56.0 <= time_s < 57.0
|
||||
or 72.0 <= time_s < 72.6
|
||||
)
|
||||
else 0.0
|
||||
),
|
||||
_constant_zero,
|
||||
),
|
||||
FlightScenario(
|
||||
"clock_drift_steps",
|
||||
"Common receiver clock-frequency steps coexist with manoeuvres.",
|
||||
90.0,
|
||||
np.asarray([120.0, 0.0, 0.0]),
|
||||
_airliner_acceleration,
|
||||
_constant_zero,
|
||||
_constant_one,
|
||||
_no_outlier,
|
||||
lambda time_s: (
|
||||
18.0 if 28.0 <= time_s < 55.0 else (-9.0 if time_s >= 55.0 else 0.0)
|
||||
),
|
||||
),
|
||||
FlightScenario(
|
||||
"gps_gap_recovery",
|
||||
"Fifteen-second GNSS gap during an aircraft manoeuvre.",
|
||||
90.0,
|
||||
np.asarray([120.0, 0.0, 0.0]),
|
||||
_airliner_acceleration,
|
||||
_constant_zero,
|
||||
_constant_one,
|
||||
_no_outlier,
|
||||
_constant_zero,
|
||||
gps_gaps=((42.0, 57.0),),
|
||||
),
|
||||
FlightScenario(
|
||||
"mixed_flight_faults",
|
||||
"Aggressive motion, temporary weak signal, channel bursts and clock step.",
|
||||
90.0,
|
||||
np.asarray([80.0, 0.0, 0.0]),
|
||||
_aggressive_acceleration,
|
||||
lambda time_s: -9.0 if 34.0 <= time_s < 60.0 else 0.0,
|
||||
lambda time_s, satellite: (
|
||||
2.5 if 34.0 <= time_s < 60.0 and satellite in (1, 6) else 1.0
|
||||
),
|
||||
lambda time_s, satellite: (
|
||||
-10.0 if satellite == 4 and 54.0 <= time_s < 55.0 else 0.0
|
||||
),
|
||||
lambda time_s: 12.0 if time_s >= 48.0 else 0.0,
|
||||
gps_gaps=((78.0, 83.0),),
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def local_los_matrix() -> np.ndarray:
|
||||
azimuth_deg = np.asarray([8.0, 55.0, 104.0, 151.0, 205.0, 253.0, 301.0, 342.0])
|
||||
elevation_deg = np.asarray([73.0, 48.0, 31.0, 18.0, 64.0, 39.0, 24.0, 14.0])
|
||||
azimuth = np.deg2rad(azimuth_deg)
|
||||
elevation = np.deg2rad(elevation_deg)
|
||||
return np.column_stack(
|
||||
[
|
||||
np.cos(elevation) * np.sin(azimuth),
|
||||
np.cos(elevation) * np.cos(azimuth),
|
||||
np.sin(elevation),
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def velocity_sigma_mps(cn0_dbhz: np.ndarray) -> np.ndarray:
|
||||
return np.clip(30.0 * np.power(10.0, -cn0_dbhz / 20.0), 0.05, 5.0)
|
||||
|
||||
|
||||
def _solve_velocity_candidate(
|
||||
design: np.ndarray,
|
||||
observations_mps: np.ndarray,
|
||||
sigma_mps: np.ndarray,
|
||||
indices: np.ndarray,
|
||||
) -> VelocityWlsResult:
|
||||
selected_design = design[indices]
|
||||
selected_observations = observations_mps[indices]
|
||||
selected_sigma = sigma_mps[indices]
|
||||
weights = 1.0 / np.square(selected_sigma)
|
||||
normal = selected_design.T @ (weights[:, None] * selected_design)
|
||||
inverse = np.linalg.inv(normal)
|
||||
solution = inverse @ (selected_design.T @ (weights * selected_observations))
|
||||
residual = selected_observations - selected_design @ solution
|
||||
weighted_residual_square_sum = float(np.sum(weights * np.square(residual)))
|
||||
degrees_of_freedom = len(indices) - 4
|
||||
covariance_scale = (
|
||||
max(1.0, weighted_residual_square_sum / degrees_of_freedom)
|
||||
if degrees_of_freedom > 0
|
||||
else 1.0
|
||||
)
|
||||
return VelocityWlsResult(
|
||||
velocity_mps=solution[:3].copy(),
|
||||
clock_drift_mps=float(solution[3]),
|
||||
covariance_mps2=covariance_scale * inverse[:3, :3],
|
||||
residual_mps=residual,
|
||||
residual_rms_mps=float(np.sqrt(np.mean(np.square(residual)))),
|
||||
covariance_scale=covariance_scale,
|
||||
excluded_satellite=None,
|
||||
)
|
||||
|
||||
|
||||
def velocity_wls(
|
||||
observations_mps: np.ndarray,
|
||||
cn0_dbhz: np.ndarray,
|
||||
*,
|
||||
robust: bool = False,
|
||||
) -> VelocityWlsResult:
|
||||
los = local_los_matrix()
|
||||
design = np.column_stack([-los, np.ones(len(los))])
|
||||
sigma = velocity_sigma_mps(cn0_dbhz)
|
||||
indices = np.arange(len(observations_mps), dtype=int)
|
||||
full = _solve_velocity_candidate(design, observations_mps, sigma, indices)
|
||||
if not robust or len(indices) < 6:
|
||||
return full
|
||||
|
||||
normalized = np.abs(full.residual_mps) / sigma
|
||||
if float(np.max(normalized)) <= 3.5:
|
||||
return full
|
||||
|
||||
# The largest full-fit residual is not necessarily the bad satellite:
|
||||
# WLS leverage can distribute one channel's error over the other rows.
|
||||
# Compare every leave-one-out hypothesis and use the one with the lowest
|
||||
# normalized post-fit residual energy. The firmware robust velocity WLS
|
||||
# uses the same trigger and candidate selection.
|
||||
candidates: list[tuple[float, int, VelocityWlsResult]] = []
|
||||
for excluded in indices:
|
||||
used = indices[indices != excluded]
|
||||
candidate = _solve_velocity_candidate(design, observations_mps, sigma, used)
|
||||
score = float(
|
||||
np.sum(np.square(candidate.residual_mps / sigma[used]))
|
||||
)
|
||||
candidates.append((score, int(excluded), candidate))
|
||||
_score, excluded, candidate = min(candidates, key=lambda item: item[0])
|
||||
return VelocityWlsResult(
|
||||
**{
|
||||
**candidate.__dict__,
|
||||
"excluded_satellite": excluded,
|
||||
}
|
||||
)
|
||||
|
||||
|
||||
def fusion_velocity_covariance(covariance_mps2: np.ndarray) -> np.ndarray:
|
||||
"""Apply fusion_gps_adapter.c's ENU velocity covariance sanitizer floors."""
|
||||
|
||||
output = np.asarray(covariance_mps2, dtype=float).copy()
|
||||
variance_floor = np.square(
|
||||
np.asarray(
|
||||
[
|
||||
MINIMUM_HORIZONTAL_VELOCITY_STD_MPS,
|
||||
MINIMUM_HORIZONTAL_VELOCITY_STD_MPS,
|
||||
MINIMUM_VERTICAL_VELOCITY_STD_MPS,
|
||||
]
|
||||
)
|
||||
)
|
||||
for axis in range(3):
|
||||
output[axis, axis] = max(output[axis, axis], variance_floor[axis])
|
||||
for row in range(3):
|
||||
for column in range(row + 1, 3):
|
||||
covariance = 0.5 * (output[row, column] + output[column, row])
|
||||
limit = 0.95 * np.sqrt(output[row, row] * output[column, column])
|
||||
covariance = float(np.clip(covariance, -limit, limit))
|
||||
output[row, column] = covariance
|
||||
output[column, row] = covariance
|
||||
return output
|
||||
|
||||
|
||||
def _in_gap(scenario: FlightScenario, time_s: float) -> bool:
|
||||
return any(start <= time_s < stop for start, stop in scenario.gps_gaps)
|
||||
|
||||
|
||||
def _truth_trajectory(scenario: FlightScenario) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
time_s = np.arange(0.0, scenario.duration_s, IMU_DT_S)
|
||||
acceleration = np.asarray([scenario.acceleration(float(time)) for time in time_s])
|
||||
velocity = np.empty((len(time_s), 3), dtype=float)
|
||||
velocity[0] = scenario.initial_velocity_mps
|
||||
velocity[1:] = velocity[0] + np.cumsum(acceleration[:-1] * IMU_DT_S, axis=0)
|
||||
return time_s, acceleration, velocity
|
||||
|
||||
|
||||
def run_doppler_eskf_scenario(
|
||||
scenario: FlightScenario,
|
||||
seed: int,
|
||||
paths: tuple[DopplerPath, ...] = PATH_ORDER,
|
||||
) -> dict[DopplerPath, DopplerEskfTrace]:
|
||||
rng = np.random.default_rng(seed)
|
||||
time_s, truth_acceleration, truth_velocity = _truth_trajectory(scenario)
|
||||
satellite_count = len(local_los_matrix())
|
||||
baseline_cn0 = np.asarray([47.0, 45.0, 43.0, 38.0, 46.0, 42.0, 40.0, 36.0])
|
||||
filtered_cn0 = baseline_cn0.copy()
|
||||
fixed_measurement: np.ndarray | None = None
|
||||
adaptive_measurement: np.ndarray | None = None
|
||||
latest_raw: np.ndarray | None = None
|
||||
alpha_selector = AdaptiveAlpha()
|
||||
latest_alpha = FIXED_DOPPLER_ALPHA
|
||||
|
||||
initial_error = rng.normal(0.0, 0.8, 3)
|
||||
filters = {
|
||||
path: VelocityBiasEskf.initialize(
|
||||
scenario.initial_velocity_mps,
|
||||
initial_error,
|
||||
)
|
||||
for path in paths
|
||||
}
|
||||
fused_history = {
|
||||
path: np.empty((len(time_s), 3), dtype=float)
|
||||
for path in paths
|
||||
}
|
||||
fused_covariance_history = {
|
||||
path: np.empty((len(time_s), 3, 3), dtype=float)
|
||||
for path in paths
|
||||
}
|
||||
wls_times = {path: [] for path in paths}
|
||||
wls_velocity = {path: [] for path in paths}
|
||||
wls_clock_drift = {path: [] for path in paths}
|
||||
wls_covariance = {path: [] for path in paths}
|
||||
wls_covariance_scale = {path: [] for path in paths}
|
||||
wls_nis = {path: [] for path in paths}
|
||||
wls_accepted = {path: [] for path in paths}
|
||||
wls_alpha = {path: [] for path in paths}
|
||||
excluded = {path: [] for path in paths}
|
||||
|
||||
true_accel_bias = rng.normal(0.0, 0.04, 3)
|
||||
latest_acceleration_measurement = truth_acceleration[0] + true_accel_bias
|
||||
los = local_los_matrix()
|
||||
|
||||
for sample in range(len(time_s)):
|
||||
if sample > 0:
|
||||
true_accel_bias += rng.normal(
|
||||
0.0,
|
||||
ACCEL_BIAS_RANDOM_WALK * np.sqrt(IMU_DT_S),
|
||||
3,
|
||||
)
|
||||
imu_noise_std = PROCESS_ACCEL_NOISE_DENSITY / np.sqrt(IMU_DT_S)
|
||||
latest_acceleration_measurement = (
|
||||
truth_acceleration[sample - 1]
|
||||
+ true_accel_bias
|
||||
+ rng.normal(0.0, imu_noise_std, 3)
|
||||
)
|
||||
for path in paths:
|
||||
filters[path].propagate(latest_acceleration_measurement, IMU_DT_S)
|
||||
|
||||
if sample % TRACKING_STEPS == 0:
|
||||
current_time = float(time_s[sample])
|
||||
true_cn0 = baseline_cn0 + scenario.cn0_offset(current_time)
|
||||
raw_cn0 = np.clip(true_cn0 + rng.normal(0.0, 1.2, satellite_count), 10.0, 60.0)
|
||||
filtered_cn0 = CN0_ALPHA * raw_cn0 + (1.0 - CN0_ALPHA) * filtered_cn0
|
||||
nominal_sigma = velocity_sigma_mps(true_cn0)
|
||||
actual_sigma = nominal_sigma * np.asarray(
|
||||
[
|
||||
scenario.noise_multiplier(current_time, satellite)
|
||||
for satellite in range(satellite_count)
|
||||
]
|
||||
)
|
||||
raw = (
|
||||
-los @ truth_velocity[sample]
|
||||
+ scenario.receiver_clock_drift_mps(current_time)
|
||||
+ rng.normal(0.0, actual_sigma)
|
||||
+ np.asarray(
|
||||
[
|
||||
scenario.outlier_mps(current_time, satellite)
|
||||
for satellite in range(satellite_count)
|
||||
]
|
||||
)
|
||||
)
|
||||
if fixed_measurement is None:
|
||||
fixed_measurement = raw.copy()
|
||||
adaptive_measurement = raw.copy()
|
||||
else:
|
||||
fixed_measurement += FIXED_DOPPLER_ALPHA * (raw - fixed_measurement)
|
||||
latest_alpha = alpha_selector.update(
|
||||
latest_acceleration_measurement,
|
||||
float(np.median(filtered_cn0)),
|
||||
)
|
||||
adaptive_measurement += latest_alpha * (raw - adaptive_measurement)
|
||||
latest_raw = raw
|
||||
|
||||
if (
|
||||
sample > 0
|
||||
and sample % WLS_STEPS == 0
|
||||
and latest_raw is not None
|
||||
and not _in_gap(scenario, float(time_s[sample]))
|
||||
):
|
||||
for path in paths:
|
||||
if path == DopplerPath.FIXED_IIR:
|
||||
observations = fixed_measurement
|
||||
elif path == DopplerPath.ADAPTIVE_IIR:
|
||||
observations = adaptive_measurement
|
||||
else:
|
||||
observations = latest_raw
|
||||
result = velocity_wls(
|
||||
np.asarray(observations),
|
||||
filtered_cn0,
|
||||
robust=(path == DopplerPath.RAW_ROBUST),
|
||||
)
|
||||
accepted = filters[path].update_velocity(
|
||||
result.velocity_mps,
|
||||
fusion_velocity_covariance(result.covariance_mps2),
|
||||
)
|
||||
wls_times[path].append(float(time_s[sample]))
|
||||
wls_velocity[path].append(result.velocity_mps)
|
||||
wls_clock_drift[path].append(result.clock_drift_mps)
|
||||
wls_covariance[path].append(result.covariance_mps2)
|
||||
wls_covariance_scale[path].append(result.covariance_scale)
|
||||
wls_nis[path].append(filters[path].last_nis)
|
||||
wls_accepted[path].append(accepted)
|
||||
wls_alpha[path].append(
|
||||
latest_alpha
|
||||
if path == DopplerPath.ADAPTIVE_IIR
|
||||
else (FIXED_DOPPLER_ALPHA if path == DopplerPath.FIXED_IIR else 1.0)
|
||||
)
|
||||
excluded[path].append(
|
||||
-1 if result.excluded_satellite is None else result.excluded_satellite
|
||||
)
|
||||
|
||||
for path in paths:
|
||||
fused_history[path][sample] = filters[path].velocity_mps
|
||||
fused_covariance_history[path][sample] = filters[path].covariance[:3, :3]
|
||||
|
||||
return {
|
||||
path: DopplerEskfTrace(
|
||||
scenario=scenario.name,
|
||||
path=path,
|
||||
time_s=time_s,
|
||||
truth_velocity_mps=truth_velocity,
|
||||
fused_velocity_mps=fused_history[path],
|
||||
fused_velocity_covariance_mps2=fused_covariance_history[path],
|
||||
acceleration_mps2=truth_acceleration,
|
||||
wls_time_s=np.asarray(wls_times[path]),
|
||||
wls_velocity_mps=np.asarray(wls_velocity[path]),
|
||||
wls_clock_drift_mps=np.asarray(wls_clock_drift[path]),
|
||||
wls_covariance_mps2=np.asarray(wls_covariance[path]),
|
||||
wls_covariance_scale=np.asarray(wls_covariance_scale[path]),
|
||||
wls_nis=np.asarray(wls_nis[path]),
|
||||
wls_accepted=np.asarray(wls_accepted[path], dtype=bool),
|
||||
adaptive_alpha=np.asarray(wls_alpha[path]),
|
||||
excluded_satellites=np.asarray(excluded[path], dtype=int),
|
||||
)
|
||||
for path in paths
|
||||
}
|
||||
@@ -0,0 +1,353 @@
|
||||
"""Python mirror of the firmware's code-measurement WLS policy.
|
||||
|
||||
The model copies the receiver-specific parts of NavEquations.c:
|
||||
|
||||
* four states: ECEF x/y/z and common receiver clock bias in metres;
|
||||
* URA + elevation + C/N0 variance model;
|
||||
* normalized weights for the numerical solve and absolute weights for
|
||||
covariance/integrity;
|
||||
* the 99-percent chi-square table;
|
||||
* single-satellite leave-one-out FDE only when at least seven measurements
|
||||
are available.
|
||||
|
||||
Atmospheric and satellite-clock corrections are represented as already
|
||||
corrected measurement errors. That keeps this study focused on tracking
|
||||
quality rather than duplicating the existing PPS orbit/atmosphere model.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
URA_SIGMA_M = np.asarray(
|
||||
[
|
||||
2.4,
|
||||
3.4,
|
||||
4.85,
|
||||
6.85,
|
||||
9.65,
|
||||
13.65,
|
||||
24.0,
|
||||
48.0,
|
||||
96.0,
|
||||
192.0,
|
||||
384.0,
|
||||
768.0,
|
||||
1536.0,
|
||||
3072.0,
|
||||
6144.0,
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
CHI_SQUARE_99 = np.asarray(
|
||||
[0.0, 6.634896601, 9.210340372, 11.34486673, 13.27670414, 15.08627247],
|
||||
dtype=float,
|
||||
)
|
||||
|
||||
|
||||
class SigmaMethod(str, Enum):
|
||||
CURRENT = "current_cn0"
|
||||
NAIVE_ADD_DLL = "naive_add_dll"
|
||||
DLL_EXCESS = "dll_excess"
|
||||
DLL_EXCESS_GUARD = "dll_excess_guard"
|
||||
ORACLE = "oracle_variance"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SigmaInputs:
|
||||
ura_index: np.ndarray
|
||||
elevation_deg: np.ndarray
|
||||
cn0_dbhz: np.ndarray
|
||||
dll_variance_ratio: np.ndarray
|
||||
dll_mean_z: np.ndarray
|
||||
actual_jitter_multiplier: np.ndarray
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class WlsSolution:
|
||||
position_ecef_m: np.ndarray
|
||||
clock_bias_m: float
|
||||
residual_m: np.ndarray
|
||||
sigma_m: np.ndarray
|
||||
covariance: np.ndarray
|
||||
chi_square: float
|
||||
chi_square_ratio: float
|
||||
chi_square_pass: bool
|
||||
valid: bool
|
||||
used_indices: np.ndarray
|
||||
excluded_index: int | None
|
||||
|
||||
|
||||
def cn0_code_sigma_m(cn0_dbhz: np.ndarray | float) -> np.ndarray:
|
||||
sigma = 3.0 * np.power(10.0, (45.0 - np.asarray(cn0_dbhz)) / 20.0)
|
||||
return np.clip(sigma, 0.5, 30.0)
|
||||
|
||||
|
||||
def elevation_sigma_m(elevation_deg: np.ndarray | float) -> np.ndarray:
|
||||
bounded = np.clip(np.asarray(elevation_deg), 10.0, 90.0)
|
||||
return 3.0 / np.sin(np.deg2rad(bounded))
|
||||
|
||||
|
||||
def firmware_sigma_m(
|
||||
inputs: SigmaInputs,
|
||||
method: SigmaMethod,
|
||||
) -> np.ndarray:
|
||||
ura = URA_SIGMA_M[np.asarray(inputs.ura_index, dtype=int)]
|
||||
elevation = elevation_sigma_m(inputs.elevation_deg)
|
||||
code = cn0_code_sigma_m(inputs.cn0_dbhz)
|
||||
ratio = np.clip(np.asarray(inputs.dll_variance_ratio), 0.05, 25.0)
|
||||
|
||||
if method == SigmaMethod.CURRENT:
|
||||
code_variance_factor = np.ones_like(code)
|
||||
elif method == SigmaMethod.NAIVE_ADD_DLL:
|
||||
# Deliberately included as a negative control: at ratio=1 this counts
|
||||
# the C/N0-predicted thermal jitter twice.
|
||||
code_variance_factor = 1.0 + ratio
|
||||
elif method == SigmaMethod.DLL_EXCESS:
|
||||
# Only the measured excess over the C/N0 expectation is added.
|
||||
code_variance_factor = np.maximum(1.0, ratio)
|
||||
elif method == SigmaMethod.DLL_EXCESS_GUARD:
|
||||
mean_factor = np.square(
|
||||
np.maximum(1.0, np.asarray(inputs.dll_mean_z) / 3.0)
|
||||
)
|
||||
code_variance_factor = np.maximum.reduce(
|
||||
[np.ones_like(code), ratio, mean_factor]
|
||||
)
|
||||
elif method == SigmaMethod.ORACLE:
|
||||
code_variance_factor = np.square(
|
||||
np.maximum(0.05, np.asarray(inputs.actual_jitter_multiplier))
|
||||
)
|
||||
else:
|
||||
raise ValueError(f"unsupported sigma method: {method}")
|
||||
|
||||
return np.sqrt(
|
||||
ura * ura
|
||||
+ elevation * elevation
|
||||
+ code * code * code_variance_factor
|
||||
)
|
||||
|
||||
|
||||
def ecef_to_lla(ecef_m: np.ndarray) -> tuple[float, float, float]:
|
||||
x, y, z = np.asarray(ecef_m, dtype=float)
|
||||
a = 6_378_137.0
|
||||
inverse_flattening = 298.257223563
|
||||
flattening = 1.0 / inverse_flattening
|
||||
eccentricity_squared = flattening * (2.0 - flattening)
|
||||
longitude = np.arctan2(y, x)
|
||||
horizontal = np.hypot(x, y)
|
||||
latitude = np.arctan2(
|
||||
z,
|
||||
horizontal * (1.0 - eccentricity_squared),
|
||||
)
|
||||
height = 0.0
|
||||
for _ in range(12):
|
||||
sine = np.sin(latitude)
|
||||
prime_vertical = a / np.sqrt(1.0 - eccentricity_squared * sine * sine)
|
||||
height = horizontal / max(np.cos(latitude), 1.0e-15) - prime_vertical
|
||||
next_latitude = np.arctan2(
|
||||
z,
|
||||
horizontal
|
||||
* (
|
||||
1.0
|
||||
- eccentricity_squared
|
||||
* prime_vertical
|
||||
/ max(prime_vertical + height, 1.0)
|
||||
),
|
||||
)
|
||||
if abs(next_latitude - latitude) < 1.0e-14:
|
||||
latitude = next_latitude
|
||||
break
|
||||
latitude = next_latitude
|
||||
return (
|
||||
float(np.rad2deg(latitude)),
|
||||
float(np.rad2deg(longitude)),
|
||||
float(height),
|
||||
)
|
||||
|
||||
|
||||
def lla_to_ecef(latitude_deg: float, longitude_deg: float, height_m: float) -> np.ndarray:
|
||||
a = 6_378_137.0
|
||||
inverse_flattening = 298.257223563
|
||||
flattening = 1.0 / inverse_flattening
|
||||
eccentricity_squared = flattening * (2.0 - flattening)
|
||||
latitude = np.deg2rad(latitude_deg)
|
||||
longitude = np.deg2rad(longitude_deg)
|
||||
sine = np.sin(latitude)
|
||||
cosine = np.cos(latitude)
|
||||
prime_vertical = a / np.sqrt(1.0 - eccentricity_squared * sine * sine)
|
||||
return np.asarray(
|
||||
[
|
||||
(prime_vertical + height_m) * cosine * np.cos(longitude),
|
||||
(prime_vertical + height_m) * cosine * np.sin(longitude),
|
||||
(
|
||||
prime_vertical * (1.0 - eccentricity_squared)
|
||||
+ height_m
|
||||
)
|
||||
* sine,
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
|
||||
|
||||
def enu_axes(receiver_ecef_m: np.ndarray) -> np.ndarray:
|
||||
latitude_deg, longitude_deg, _ = ecef_to_lla(receiver_ecef_m)
|
||||
latitude = np.deg2rad(latitude_deg)
|
||||
longitude = np.deg2rad(longitude_deg)
|
||||
east = np.asarray([-np.sin(longitude), np.cos(longitude), 0.0])
|
||||
north = np.asarray(
|
||||
[
|
||||
-np.sin(latitude) * np.cos(longitude),
|
||||
-np.sin(latitude) * np.sin(longitude),
|
||||
np.cos(latitude),
|
||||
]
|
||||
)
|
||||
up = np.asarray(
|
||||
[
|
||||
np.cos(latitude) * np.cos(longitude),
|
||||
np.cos(latitude) * np.sin(longitude),
|
||||
np.sin(latitude),
|
||||
]
|
||||
)
|
||||
return np.vstack([east, north, up])
|
||||
|
||||
|
||||
def satellites_from_azel(
|
||||
receiver_ecef_m: np.ndarray,
|
||||
azimuth_deg: np.ndarray,
|
||||
elevation_deg: np.ndarray,
|
||||
range_m: float = 21_000_000.0,
|
||||
) -> np.ndarray:
|
||||
axes = enu_axes(receiver_ecef_m)
|
||||
satellites: list[np.ndarray] = []
|
||||
for azimuth, elevation in zip(azimuth_deg, elevation_deg):
|
||||
azimuth_rad = np.deg2rad(azimuth)
|
||||
elevation_rad = np.deg2rad(elevation)
|
||||
los_enu = np.asarray(
|
||||
[
|
||||
np.cos(elevation_rad) * np.sin(azimuth_rad),
|
||||
np.cos(elevation_rad) * np.cos(azimuth_rad),
|
||||
np.sin(elevation_rad),
|
||||
]
|
||||
)
|
||||
los_ecef = axes.T @ los_enu
|
||||
satellites.append(receiver_ecef_m + range_m * los_ecef)
|
||||
return np.asarray(satellites)
|
||||
|
||||
|
||||
def position_error_enu_m(
|
||||
estimated_ecef_m: np.ndarray,
|
||||
true_ecef_m: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
return enu_axes(true_ecef_m) @ (estimated_ecef_m - true_ecef_m)
|
||||
|
||||
|
||||
def _candidate(
|
||||
satellite_positions_m: np.ndarray,
|
||||
observations_m: np.ndarray,
|
||||
sigma_m: np.ndarray,
|
||||
initial_state: np.ndarray,
|
||||
used_indices: np.ndarray,
|
||||
) -> WlsSolution:
|
||||
state = np.asarray(initial_state, dtype=float).copy()
|
||||
weights = 1.0 / np.square(sigma_m)
|
||||
design = np.empty((len(observations_m), 4), dtype=float)
|
||||
|
||||
for _ in range(10):
|
||||
difference = satellite_positions_m - state[:3]
|
||||
geometric_range = np.linalg.norm(difference, axis=1)
|
||||
design[:, :3] = -difference / geometric_range[:, None]
|
||||
design[:, 3] = 1.0
|
||||
residual = observations_m - geometric_range - state[3]
|
||||
normal = design.T @ (weights[:, None] * design)
|
||||
rhs = design.T @ (weights * residual)
|
||||
update = np.linalg.solve(normal, rhs)
|
||||
state += update
|
||||
if np.linalg.norm(update) < 1.0e-4:
|
||||
break
|
||||
|
||||
difference = satellite_positions_m - state[:3]
|
||||
geometric_range = np.linalg.norm(difference, axis=1)
|
||||
design[:, :3] = -difference / geometric_range[:, None]
|
||||
design[:, 3] = 1.0
|
||||
residual = observations_m - geometric_range - state[3]
|
||||
normal = design.T @ (weights[:, None] * design)
|
||||
covariance = np.linalg.inv(normal)
|
||||
chi_square = float(np.sum(weights * np.square(residual)))
|
||||
degrees_of_freedom = len(observations_m) - 4
|
||||
threshold = (
|
||||
float(CHI_SQUARE_99[degrees_of_freedom])
|
||||
if 0 < degrees_of_freedom < len(CHI_SQUARE_99)
|
||||
else 0.0
|
||||
)
|
||||
chi_square_pass = degrees_of_freedom > 0 and chi_square <= threshold
|
||||
valid = (
|
||||
(len(observations_m) == 4 and degrees_of_freedom == 0)
|
||||
or chi_square_pass
|
||||
)
|
||||
ratio = chi_square / threshold if threshold > 0.0 else float("inf")
|
||||
return WlsSolution(
|
||||
position_ecef_m=state[:3].copy(),
|
||||
clock_bias_m=float(state[3]),
|
||||
residual_m=residual.copy(),
|
||||
sigma_m=sigma_m.copy(),
|
||||
covariance=covariance,
|
||||
chi_square=chi_square,
|
||||
chi_square_ratio=ratio,
|
||||
chi_square_pass=chi_square_pass,
|
||||
valid=valid,
|
||||
used_indices=used_indices.copy(),
|
||||
excluded_index=None,
|
||||
)
|
||||
|
||||
|
||||
def firmware_wls(
|
||||
satellite_positions_m: np.ndarray,
|
||||
observations_m: np.ndarray,
|
||||
sigma_m: np.ndarray,
|
||||
initial_state: np.ndarray,
|
||||
*,
|
||||
enable_fde: bool = True,
|
||||
) -> WlsSolution:
|
||||
count = len(observations_m)
|
||||
if count < 4:
|
||||
raise ValueError("position WLS needs at least four satellites")
|
||||
full_indices = np.arange(count, dtype=int)
|
||||
full = _candidate(
|
||||
satellite_positions_m,
|
||||
observations_m,
|
||||
sigma_m,
|
||||
initial_state,
|
||||
full_indices,
|
||||
)
|
||||
if full.chi_square_pass or not enable_fde or count < 7:
|
||||
return full
|
||||
|
||||
selected = full
|
||||
passing_found = False
|
||||
full_state = np.concatenate([full.position_ecef_m, [full.clock_bias_m]])
|
||||
for excluded in range(count):
|
||||
used = full_indices[full_indices != excluded]
|
||||
candidate = _candidate(
|
||||
satellite_positions_m[used],
|
||||
observations_m[used],
|
||||
sigma_m[used],
|
||||
full_state,
|
||||
used,
|
||||
)
|
||||
candidate = WlsSolution(
|
||||
**{
|
||||
**candidate.__dict__,
|
||||
"excluded_index": excluded,
|
||||
}
|
||||
)
|
||||
if candidate.chi_square_pass:
|
||||
if not passing_found or candidate.chi_square_ratio < selected.chi_square_ratio:
|
||||
selected = candidate
|
||||
passing_found = True
|
||||
elif not passing_found and candidate.chi_square_ratio < selected.chi_square_ratio:
|
||||
selected = candidate
|
||||
return selected
|
||||
@@ -0,0 +1,368 @@
|
||||
"""Synthetic code-tracking scenarios shared by the analysis and tests."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Callable
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .firmware_wls_model import (
|
||||
SigmaInputs,
|
||||
SigmaMethod,
|
||||
URA_SIGMA_M,
|
||||
cn0_code_sigma_m,
|
||||
elevation_sigma_m,
|
||||
enu_axes,
|
||||
firmware_sigma_m,
|
||||
firmware_wls,
|
||||
lla_to_ecef,
|
||||
position_error_enu_m,
|
||||
satellites_from_azel,
|
||||
)
|
||||
from .rtl_dll_model import expected_discriminator_std, synthesize_dll_window
|
||||
|
||||
|
||||
TRACKING_PERIOD_S = 0.020
|
||||
WLS_PERIOD_S = 0.200
|
||||
TRACKING_UPDATES_PER_WLS = int(round(WLS_PERIOD_S / TRACKING_PERIOD_S))
|
||||
FIRMWARE_CN0_ALPHA = 0.20
|
||||
DLL_RATIO_ALPHA = 0.10
|
||||
DLL_MEAN_ALPHA = 0.20
|
||||
|
||||
GOOD_AZIMUTH_DEG = np.asarray([8.0, 55.0, 104.0, 151.0, 205.0, 253.0, 301.0, 342.0])
|
||||
GOOD_ELEVATION_DEG = np.asarray([73.0, 48.0, 31.0, 18.0, 64.0, 39.0, 24.0, 14.0])
|
||||
POOR_AZIMUTH_DEG = np.asarray([25.0, 46.0, 68.0, 89.0, 112.0, 136.0, 159.0, 182.0])
|
||||
POOR_ELEVATION_DEG = np.asarray([18.0, 22.0, 25.0, 29.0, 32.0, 27.0, 21.0, 16.0])
|
||||
|
||||
|
||||
Profile = Callable[[float, int, float], float]
|
||||
|
||||
|
||||
def _identity(_time_s: float, _satellite: int, baseline: float) -> float:
|
||||
return baseline
|
||||
|
||||
|
||||
def _one(_time_s: float, _satellite: int, _baseline: float) -> float:
|
||||
return 1.0
|
||||
|
||||
|
||||
def _zero(_time_s: float, _satellite: int, _baseline: float) -> float:
|
||||
return 0.0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Scenario:
|
||||
name: str
|
||||
description: str
|
||||
duration_s: float = 80.0
|
||||
poor_geometry: bool = False
|
||||
cn0_profile: Profile = _identity
|
||||
jitter_profile: Profile = _one
|
||||
discriminator_bias_profile: Profile = _zero
|
||||
pseudorange_bias_profile: Profile = _zero
|
||||
|
||||
|
||||
@dataclass
|
||||
class ChannelQuality:
|
||||
cn0_dbhz: float | None = None
|
||||
dll_ratio: float = 1.0
|
||||
dll_mean_z: float = 0.0
|
||||
|
||||
def update(
|
||||
self,
|
||||
raw_cn0_dbhz: float,
|
||||
dll_samples: np.ndarray,
|
||||
) -> None:
|
||||
if self.cn0_dbhz is None:
|
||||
self.cn0_dbhz = raw_cn0_dbhz
|
||||
else:
|
||||
self.cn0_dbhz = (
|
||||
FIRMWARE_CN0_ALPHA * raw_cn0_dbhz
|
||||
+ (1.0 - FIRMWARE_CN0_ALPHA) * self.cn0_dbhz
|
||||
)
|
||||
|
||||
measured_variance = float(np.var(dll_samples, ddof=1))
|
||||
expected_std = expected_discriminator_std(self.cn0_dbhz)
|
||||
raw_ratio = measured_variance / max(expected_std * expected_std, 1.0e-15)
|
||||
raw_ratio = float(np.clip(raw_ratio, 0.05, 25.0))
|
||||
self.dll_ratio = (
|
||||
DLL_RATIO_ALPHA * raw_ratio
|
||||
+ (1.0 - DLL_RATIO_ALPHA) * self.dll_ratio
|
||||
)
|
||||
|
||||
standard_error = np.sqrt(
|
||||
max(measured_variance, expected_std * expected_std)
|
||||
/ len(dll_samples)
|
||||
)
|
||||
raw_mean_z = abs(float(np.mean(dll_samples))) / max(standard_error, 1.0e-12)
|
||||
raw_mean_z = float(np.clip(raw_mean_z, 0.0, 12.0))
|
||||
self.dll_mean_z = (
|
||||
DLL_MEAN_ALPHA * raw_mean_z
|
||||
+ (1.0 - DLL_MEAN_ALPHA) * self.dll_mean_z
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EpochSample:
|
||||
method: SigmaMethod
|
||||
time_s: float
|
||||
enu_error_m: np.ndarray
|
||||
horizontal_error_m: float
|
||||
position_error_3d_m: float
|
||||
horizontal_sigma_m: float
|
||||
valid: bool
|
||||
chi_square_ratio: float
|
||||
fde_applied: bool
|
||||
mean_measurement_sigma_m: float
|
||||
maximum_dll_ratio: float
|
||||
|
||||
|
||||
def scenario_matrix() -> tuple[Scenario, ...]:
|
||||
return (
|
||||
Scenario(
|
||||
"clean_open_sky",
|
||||
"C/N0 model and actual DLL jitter agree.",
|
||||
),
|
||||
Scenario(
|
||||
"weak_signal",
|
||||
"All channels lose 11 dB; current C/N0 weighting already sees it.",
|
||||
cn0_profile=lambda _t, _s, baseline: baseline - 11.0,
|
||||
),
|
||||
Scenario(
|
||||
"one_channel_hidden_jitter",
|
||||
"One high-C/N0 channel has 4x DLL jitter not visible in C/N0.",
|
||||
jitter_profile=lambda _t, satellite, _b: 4.0 if satellite == 1 else 1.0,
|
||||
),
|
||||
Scenario(
|
||||
"common_hidden_jitter",
|
||||
"All channels have the same unmodelled 3x DLL jitter.",
|
||||
jitter_profile=lambda _t, _satellite, _b: 3.0,
|
||||
),
|
||||
Scenario(
|
||||
"noise_burst_recovery",
|
||||
"Two channels have 5x jitter from 22 s to 45 s, then recover.",
|
||||
jitter_profile=lambda time_s, satellite, _b: (
|
||||
5.0
|
||||
if 22.0 <= time_s < 45.0 and satellite in (1, 5)
|
||||
else 1.0
|
||||
),
|
||||
),
|
||||
Scenario(
|
||||
"dll_transient_stress",
|
||||
"A channel has discriminator mean without excess variance during transient stress.",
|
||||
discriminator_bias_profile=lambda time_s, satellite, _b: (
|
||||
0.055
|
||||
if 25.0 <= time_s < 34.0 and satellite == 3
|
||||
else 0.0
|
||||
),
|
||||
pseudorange_bias_profile=lambda time_s, satellite, _b: (
|
||||
9.0
|
||||
if 25.0 <= time_s < 34.0 and satellite == 3
|
||||
else 0.0
|
||||
),
|
||||
),
|
||||
Scenario(
|
||||
"stable_multipath_bias",
|
||||
"Low-jitter, high-C/N0 channel has a stable 14 m code bias.",
|
||||
pseudorange_bias_profile=lambda _t, satellite, _b: (
|
||||
14.0 if satellite == 6 else 0.0
|
||||
),
|
||||
),
|
||||
Scenario(
|
||||
"mixed_multipath",
|
||||
"One channel has both a 10 m bias and 3x excess jitter.",
|
||||
jitter_profile=lambda _t, satellite, _b: 3.0 if satellite == 2 else 1.0,
|
||||
pseudorange_bias_profile=lambda _t, satellite, _b: (
|
||||
10.0 if satellite == 2 else 0.0
|
||||
),
|
||||
),
|
||||
Scenario(
|
||||
"poor_geometry_hidden_jitter",
|
||||
"Clustered geometry plus one channel with unmodelled 4x jitter.",
|
||||
poor_geometry=True,
|
||||
jitter_profile=lambda _t, satellite, _b: 4.0 if satellite == 4 else 1.0,
|
||||
),
|
||||
Scenario(
|
||||
"cn0_step_recovery",
|
||||
"C/N0 is initially weak and improves; no hidden DLL noise exists.",
|
||||
cn0_profile=lambda time_s, _satellite, baseline: (
|
||||
baseline - 12.0 if time_s < 30.0 else baseline
|
||||
),
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def _geometry(scenario: Scenario) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
receiver = lla_to_ecef(39.9208, 32.8541, 930.0)
|
||||
if scenario.poor_geometry:
|
||||
azimuth = POOR_AZIMUTH_DEG
|
||||
elevation = POOR_ELEVATION_DEG
|
||||
else:
|
||||
azimuth = GOOD_AZIMUTH_DEG
|
||||
elevation = GOOD_ELEVATION_DEG
|
||||
satellites = satellites_from_azel(receiver, azimuth, elevation)
|
||||
return receiver, satellites, elevation.copy()
|
||||
|
||||
|
||||
def run_scenario(
|
||||
scenario: Scenario,
|
||||
seed: int,
|
||||
methods: tuple[SigmaMethod, ...] = tuple(SigmaMethod),
|
||||
) -> list[EpochSample]:
|
||||
rng = np.random.default_rng(seed)
|
||||
receiver, satellites, elevations = _geometry(scenario)
|
||||
satellite_count = len(satellites)
|
||||
geometric_ranges = np.linalg.norm(satellites - receiver, axis=1)
|
||||
baseline_cn0 = np.clip(34.0 + 0.16 * elevations, 30.0, 48.0)
|
||||
ura_indices = np.zeros(satellite_count, dtype=np.uint8)
|
||||
quality = [ChannelQuality() for _ in range(satellite_count)]
|
||||
|
||||
# These are deliberately slower/non-DLL terms. They prevent a synthetic
|
||||
# white-noise-only experiment from overstating what DLL weighting can fix.
|
||||
ura_error = rng.normal(0.0, 0.45 * URA_SIGMA_M[0], satellite_count)
|
||||
atmospheric_state = rng.normal(
|
||||
0.0,
|
||||
0.35 * elevation_sigma_m(elevations),
|
||||
satellite_count,
|
||||
)
|
||||
code_noise_state = np.zeros(satellite_count)
|
||||
initial_state = np.concatenate(
|
||||
[
|
||||
receiver + np.asarray([450.0, -300.0, 220.0]),
|
||||
[0.0],
|
||||
]
|
||||
)
|
||||
method_state = {method: initial_state.copy() for method in methods}
|
||||
samples: list[EpochSample] = []
|
||||
clock_bias_m = 82.0
|
||||
receiver_enu_axes = enu_axes(receiver)
|
||||
|
||||
tracking_steps = int(round(scenario.duration_s / TRACKING_PERIOD_S))
|
||||
latest_actual_jitter = np.ones(satellite_count)
|
||||
for tracking_step in range(tracking_steps):
|
||||
time_s = tracking_step * TRACKING_PERIOD_S
|
||||
for satellite in range(satellite_count):
|
||||
true_cn0 = scenario.cn0_profile(
|
||||
time_s,
|
||||
satellite,
|
||||
float(baseline_cn0[satellite]),
|
||||
)
|
||||
actual_jitter = scenario.jitter_profile(
|
||||
time_s,
|
||||
satellite,
|
||||
1.0,
|
||||
)
|
||||
discriminator_bias = scenario.discriminator_bias_profile(
|
||||
time_s,
|
||||
satellite,
|
||||
0.0,
|
||||
)
|
||||
latest_actual_jitter[satellite] = actual_jitter
|
||||
dll_window = synthesize_dll_window(
|
||||
rng,
|
||||
cn0_dbhz=true_cn0,
|
||||
jitter_multiplier=actual_jitter,
|
||||
discriminator_bias=discriminator_bias,
|
||||
)
|
||||
# The hardware NWPR C/N0 estimate is noisy; firmware applies alpha=0.2.
|
||||
raw_cn0 = float(np.clip(true_cn0 + rng.normal(0.0, 1.3), 10.0, 60.0))
|
||||
quality[satellite].update(raw_cn0, dll_window.samples)
|
||||
|
||||
if (tracking_step + 1) % TRACKING_UPDATES_PER_WLS != 0:
|
||||
continue
|
||||
|
||||
true_cn0 = np.asarray(
|
||||
[
|
||||
scenario.cn0_profile(
|
||||
time_s,
|
||||
satellite,
|
||||
float(baseline_cn0[satellite]),
|
||||
)
|
||||
for satellite in range(satellite_count)
|
||||
]
|
||||
)
|
||||
actual_code_sigma = cn0_code_sigma_m(true_cn0) * latest_actual_jitter
|
||||
innovation = rng.normal(0.0, actual_code_sigma)
|
||||
code_noise_state = (
|
||||
0.25 * code_noise_state
|
||||
+ np.sqrt(1.0 - 0.25**2) * innovation
|
||||
)
|
||||
atmospheric_innovation = rng.normal(
|
||||
0.0,
|
||||
0.35 * elevation_sigma_m(elevations),
|
||||
)
|
||||
atmospheric_state = (
|
||||
0.97 * atmospheric_state
|
||||
+ np.sqrt(1.0 - 0.97**2) * atmospheric_innovation
|
||||
)
|
||||
pseudorange_bias = np.asarray(
|
||||
[
|
||||
scenario.pseudorange_bias_profile(
|
||||
time_s,
|
||||
satellite,
|
||||
0.0,
|
||||
)
|
||||
for satellite in range(satellite_count)
|
||||
]
|
||||
)
|
||||
observations = (
|
||||
geometric_ranges
|
||||
+ clock_bias_m
|
||||
+ ura_error
|
||||
+ atmospheric_state
|
||||
+ code_noise_state
|
||||
+ pseudorange_bias
|
||||
)
|
||||
sigma_inputs = SigmaInputs(
|
||||
ura_index=ura_indices,
|
||||
elevation_deg=elevations,
|
||||
cn0_dbhz=np.asarray([item.cn0_dbhz for item in quality], dtype=float),
|
||||
dll_variance_ratio=np.asarray([item.dll_ratio for item in quality]),
|
||||
dll_mean_z=np.asarray([item.dll_mean_z for item in quality]),
|
||||
actual_jitter_multiplier=latest_actual_jitter.copy(),
|
||||
)
|
||||
|
||||
for method in methods:
|
||||
sigma = firmware_sigma_m(sigma_inputs, method)
|
||||
solution = firmware_wls(
|
||||
satellites,
|
||||
observations,
|
||||
sigma,
|
||||
method_state[method],
|
||||
)
|
||||
# NavEquations.c updates its persistent WLS seed only after an
|
||||
# integrity-valid solution.
|
||||
if solution.valid:
|
||||
method_state[method] = np.concatenate(
|
||||
[solution.position_ecef_m, [solution.clock_bias_m]]
|
||||
)
|
||||
enu_error = position_error_enu_m(solution.position_ecef_m, receiver)
|
||||
enu_covariance = (
|
||||
# The top-left block is ECEF position covariance.
|
||||
# R * Q * R' maps it to ENU.
|
||||
receiver_enu_axes
|
||||
@ solution.covariance[:3, :3]
|
||||
@ receiver_enu_axes.T
|
||||
)
|
||||
horizontal_sigma = float(
|
||||
np.sqrt(max(0.0, enu_covariance[0, 0] + enu_covariance[1, 1]))
|
||||
)
|
||||
samples.append(
|
||||
EpochSample(
|
||||
method=method,
|
||||
time_s=time_s,
|
||||
enu_error_m=enu_error,
|
||||
horizontal_error_m=float(np.linalg.norm(enu_error[:2])),
|
||||
position_error_3d_m=float(np.linalg.norm(enu_error)),
|
||||
horizontal_sigma_m=horizontal_sigma,
|
||||
valid=solution.valid,
|
||||
chi_square_ratio=solution.chi_square_ratio,
|
||||
fde_applied=solution.excluded_index is not None,
|
||||
mean_measurement_sigma_m=float(np.mean(sigma)),
|
||||
maximum_dll_ratio=float(
|
||||
max(item.dll_ratio for item in quality)
|
||||
),
|
||||
)
|
||||
)
|
||||
return samples
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 86 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 92 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 213 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 41 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 84 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 176 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 184 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 352 KiB |
@@ -0,0 +1,94 @@
|
||||
# GPS code measurement model sonuçları
|
||||
|
||||
Monte Carlo seed sayısı: `12`. İlk 5 saniye metriklere alınmadı.
|
||||
|
||||
## Yöntem karşılaştırması
|
||||
|
||||
| Senaryo | Yöntem | 3D RMS (m) | Mevcuda göre | P95 (m) | Valid (%) | >25 m false-valid (%) |
|
||||
|---|---|---:|---:|---:|---:|---:|
|
||||
| clean_open_sky | Mevcut C/N0 | 10.05 | +0.0% | 17.35 | 100.0 | 0.31 |
|
||||
| clean_open_sky | Naif DLL ekle | 10.02 | +0.3% | 17.25 | 100.0 | 0.31 |
|
||||
| clean_open_sky | DLL excess | 10.05 | +0.0% | 17.32 | 100.0 | 0.33 |
|
||||
| clean_open_sky | DLL excess+mean | 10.05 | +0.0% | 17.32 | 100.0 | 0.33 |
|
||||
| clean_open_sky | Oracle variance | 10.05 | +0.0% | 17.35 | 100.0 | 0.31 |
|
||||
| weak_signal | Mevcut C/N0 | 32.73 | +0.0% | 56.44 | 100.0 | 56.31 |
|
||||
| weak_signal | Naif DLL ekle | 32.31 | +1.3% | 55.46 | 100.0 | 56.18 |
|
||||
| weak_signal | DLL excess | 32.65 | +0.3% | 56.12 | 100.0 | 56.22 |
|
||||
| weak_signal | DLL excess+mean | 32.65 | +0.3% | 56.12 | 100.0 | 56.22 |
|
||||
| weak_signal | Oracle variance | 32.73 | +0.0% | 56.44 | 100.0 | 56.31 |
|
||||
| one_channel_hidden_jitter | Mevcut C/N0 | 12.27 | +0.0% | 19.94 | 100.0 | 1.07 |
|
||||
| one_channel_hidden_jitter | Naif DLL ekle | 10.49 | +14.5% | 17.75 | 100.0 | 0.33 |
|
||||
| one_channel_hidden_jitter | DLL excess | 10.47 | +14.7% | 17.68 | 100.0 | 0.33 |
|
||||
| one_channel_hidden_jitter | DLL excess+mean | 10.47 | +14.7% | 17.68 | 100.0 | 0.33 |
|
||||
| one_channel_hidden_jitter | Oracle variance | 10.46 | +14.7% | 17.74 | 100.0 | 0.33 |
|
||||
| common_hidden_jitter | Mevcut C/N0 | 33.79 | +0.0% | 59.76 | 94.8 | 52.40 |
|
||||
| common_hidden_jitter | Naif DLL ekle | 27.61 | +18.3% | 47.56 | 100.0 | 43.93 |
|
||||
| common_hidden_jitter | DLL excess | 27.69 | +18.1% | 47.83 | 100.0 | 43.89 |
|
||||
| common_hidden_jitter | DLL excess+mean | 27.69 | +18.1% | 47.83 | 100.0 | 43.89 |
|
||||
| common_hidden_jitter | Oracle variance | 27.74 | +17.9% | 47.83 | 100.0 | 43.87 |
|
||||
| noise_burst_recovery | Mevcut C/N0 | 12.96 | +0.0% | 22.13 | 98.9 | 2.58 |
|
||||
| noise_burst_recovery | Naif DLL ekle | 10.52 | +18.8% | 18.03 | 100.0 | 0.53 |
|
||||
| noise_burst_recovery | DLL excess | 10.49 | +19.0% | 18.00 | 100.0 | 0.49 |
|
||||
| noise_burst_recovery | DLL excess+mean | 10.49 | +19.0% | 18.00 | 100.0 | 0.49 |
|
||||
| noise_burst_recovery | Oracle variance | 10.47 | +19.2% | 17.90 | 100.0 | 0.49 |
|
||||
| dll_transient_stress | Mevcut C/N0 | 10.15 | +0.0% | 17.43 | 100.0 | 0.36 |
|
||||
| dll_transient_stress | Naif DLL ekle | 10.12 | +0.3% | 17.33 | 100.0 | 0.38 |
|
||||
| dll_transient_stress | DLL excess | 10.14 | +0.0% | 17.38 | 100.0 | 0.38 |
|
||||
| dll_transient_stress | DLL excess+mean | 10.12 | +0.2% | 17.37 | 100.0 | 0.33 |
|
||||
| dll_transient_stress | Oracle variance | 10.15 | +0.0% | 17.43 | 100.0 | 0.36 |
|
||||
| stable_multipath_bias | Mevcut C/N0 | 12.55 | +0.0% | 21.00 | 100.0 | 1.67 |
|
||||
| stable_multipath_bias | Naif DLL ekle | 12.39 | +1.3% | 20.87 | 100.0 | 1.47 |
|
||||
| stable_multipath_bias | DLL excess | 12.54 | +0.1% | 21.02 | 100.0 | 1.64 |
|
||||
| stable_multipath_bias | DLL excess+mean | 12.54 | +0.1% | 21.02 | 100.0 | 1.64 |
|
||||
| stable_multipath_bias | Oracle variance | 12.55 | +0.0% | 21.00 | 100.0 | 1.67 |
|
||||
| mixed_multipath | Mevcut C/N0 | 14.43 | +0.0% | 25.38 | 100.0 | 5.64 |
|
||||
| mixed_multipath | Naif DLL ekle | 11.26 | +22.0% | 19.60 | 100.0 | 0.73 |
|
||||
| mixed_multipath | DLL excess | 11.07 | +23.3% | 19.23 | 100.0 | 0.73 |
|
||||
| mixed_multipath | DLL excess+mean | 11.07 | +23.3% | 19.23 | 100.0 | 0.73 |
|
||||
| mixed_multipath | Oracle variance | 11.06 | +23.4% | 19.20 | 100.0 | 0.73 |
|
||||
| poor_geometry_hidden_jitter | Mevcut C/N0 | 293.82 | +0.0% | 574.96 | 100.0 | 94.31 |
|
||||
| poor_geometry_hidden_jitter | Naif DLL ekle | 206.13 | +29.8% | 405.15 | 100.0 | 92.42 |
|
||||
| poor_geometry_hidden_jitter | DLL excess | 196.87 | +33.0% | 390.64 | 100.0 | 91.40 |
|
||||
| poor_geometry_hidden_jitter | DLL excess+mean | 196.87 | +33.0% | 390.64 | 100.0 | 91.40 |
|
||||
| poor_geometry_hidden_jitter | Oracle variance | 196.20 | +33.2% | 386.58 | 100.0 | 91.27 |
|
||||
| cn0_step_recovery | Mevcut C/N0 | 22.59 | +0.0% | 47.95 | 100.0 | 21.62 |
|
||||
| cn0_step_recovery | Naif DLL ekle | 22.27 | +1.4% | 47.59 | 100.0 | 21.42 |
|
||||
| cn0_step_recovery | DLL excess | 22.59 | -0.0% | 47.91 | 100.0 | 21.49 |
|
||||
| cn0_step_recovery | DLL excess+mean | 22.59 | -0.0% | 47.91 | 100.0 | 21.49 |
|
||||
| cn0_step_recovery | Oracle variance | 22.59 | +0.0% | 47.95 | 100.0 | 21.62 |
|
||||
|
||||
## Modelden çıkan karar özeti
|
||||
|
||||
- C/N0 ile uyumlu nominal DLL jitter'da `dll_excess`, mevcut çözümü değiştirmemelidir.
|
||||
- Bütün measurement sigmaları aynı sabitle çarpılırsa WLS konum noktası değişmez; yalnız covariance/integrity değişir. Fakat ortak DLL jitter çarpanı, C/N0/elevation bileşimi kanaldan kanala farklı olduğu için toplam sigmaları eşit oranda büyütmez.
|
||||
- Kanal-bağımlı ve C/N0'da görünmeyen excess jitter varsa dinamik sigma o kanalı down-weight ederek konumu iyileştirebilir.
|
||||
- Stable multipath bias düşük DLL variance bırakabildiği için variance-only yöntem çözüm değildir.
|
||||
- 50 Hz'de DLL ratio alpha=0.10 yaklaşık 0.19 s zaman sabiti ve 0.57 s yüzde-95 geçiş verir.
|
||||
- Aşağıdaki yüzdeler seçilen sentetik hata büyüklüklerine koşulludur; kart için kazanç tahmini değildir.
|
||||
|
||||
## Bozulma penceresi odaklı 3D RMS
|
||||
|
||||
| Senaryo/pencere | Mevcut (m) | DLL excess (m) | DLL excess+mean (m) | Oracle variance (m) |
|
||||
|---|---:|---:|---:|---:|
|
||||
| noise_burst_recovery [22,45) s | 17.83 | 11.36 | 11.36 | 11.30 |
|
||||
| dll_transient_stress [25,34) s | 10.83 | 10.82 | 10.65 | 10.83 |
|
||||
| cn0_step_recovery [5,30) s | 36.48 | 36.48 | 36.48 | 36.48 |
|
||||
|
||||
## Code smoothing ve carrier CFO
|
||||
|
||||
| Carrier-rate koşulu | Ham RMSE (m) | Kör IIR RMSE (m) | Raw Doppler RMSE (m) | FW-IIR Doppler RMSE (m) | Kör IIR peak (m) | Raw Doppler peak (m) | FW-IIR Doppler peak (m) |
|
||||
|---|---:|---:|---:|---:|---:|---:|---:|
|
||||
| cfo_compensated | 3.99 | 248.61 | 0.94 | 16.42 | 414.08 | 2.05 | 84.13 |
|
||||
| cfo_residual_0p5_mps | 3.99 | 248.61 | 1.30 | 16.41 | 414.08 | 2.57 | 83.23 |
|
||||
| cfo_residual_3_mps | 3.99 | 248.61 | 5.48 | 17.06 | 414.08 | 7.07 | 78.73 |
|
||||
|
||||
## Yorum sınırları
|
||||
|
||||
- `current_cn0`, firmware'deki URA/elevation/C/N0 sigma modelidir.
|
||||
- `naive_add_dll`, aynı thermal jitter'ı iki kere saymanın negatif kontrolüdür.
|
||||
- `dll_excess`, yalnız ölçülen DLL variance'ın C/N0 beklentisini aşan kısmını code variance'a ekler.
|
||||
- `dll_excess_guard`, variance'a ek olarak sürekli discriminator mean stress'ini kullanır.
|
||||
- `oracle_variance`, sentetik gerçek jitter çarpanını bilir; yalnız variance kaynakları için erişilebilir üst sınırdır, bias'ı bilmez.
|
||||
- Stable multipath DLL'nin yeni denge noktasında düşük residual variance bırakabilir. Bu nedenle variance-only yönteminin onu çözmesi beklenmez.
|
||||
- Discriminator gürültü ölçeği kart correlator kayıtları gelene kadar kalibrasyon varsayımıdır; oran tabanlı yöntem mutlak ölçeğe göre daha az hassastır.
|
||||
- Sonuçlar bitstream veya firmware değişikliği değildir; karar öncesi executable tasarım keşfidir.
|
||||
@@ -0,0 +1,65 @@
|
||||
# Doppler WLS -> ESKF model sonuçları
|
||||
|
||||
Monte Carlo seed sayısı: `20`; ilk 5 saniye RMS metriklerinden çıkarıldı.
|
||||
50 Hz tracking, önceki `alpha=0.05` Doppler IIR, 5 Hz dört-state velocity WLS, fusion adapter'ın `2/2/3 m/s` ENU sigma tabanları, TDK profiliyle 100 Hz IMU propagation ve 3-DOF `%99.9` NIS kapısı modellenmiştir.
|
||||
|
||||
## Sonuç tablosu
|
||||
|
||||
| Senaryo | Yol | ESKF RMS (m/s) | Dynamic RMS | WLS RMS | Accept (%) | 95% cov. (%) | Exclude (%) |
|
||||
|---|---|---:|---:|---:|---:|---:|---:|
|
||||
| steady_cruise | Önceki sabit IIR | 0.182 | - | 0.068 | 100.0 | 100.0 | 0.0 |
|
||||
| steady_cruise | Ham Doppler + ESKF | 0.190 | - | 0.416 | 100.0 | 100.0 | 0.0 |
|
||||
| steady_cruise | Adaptif IIR + ESKF | 0.182 | - | 0.068 | 100.0 | 100.0 | 0.0 |
|
||||
| steady_cruise | Ham + robust WLS + ESKF | 0.190 | - | 0.416 | 100.0 | 100.0 | 0.0 |
|
||||
| airliner_maneuver | Önceki sabit IIR | 0.416 | 0.438 | 0.440 | 100.0 | 99.7 | 0.0 |
|
||||
| airliner_maneuver | Ham Doppler + ESKF | 0.190 | 0.179 | 0.416 | 100.0 | 100.0 | 0.0 |
|
||||
| airliner_maneuver | Adaptif IIR + ESKF | 0.194 | 0.189 | 0.135 | 100.0 | 100.0 | 0.0 |
|
||||
| airliner_maneuver | Ham + robust WLS + ESKF | 0.190 | 0.179 | 0.416 | 100.0 | 100.0 | 0.0 |
|
||||
| aggressive_maneuver | Önceki sabit IIR | 0.942 | 0.919 | 1.178 | 100.0 | 55.9 | 0.0 |
|
||||
| aggressive_maneuver | Ham Doppler + ESKF | 0.190 | 0.186 | 0.416 | 100.0 | 100.0 | 0.0 |
|
||||
| aggressive_maneuver | Adaptif IIR + ESKF | 0.200 | 0.197 | 0.171 | 100.0 | 100.0 | 0.0 |
|
||||
| aggressive_maneuver | Ham + robust WLS + ESKF | 0.190 | 0.186 | 0.416 | 100.0 | 100.0 | 0.0 |
|
||||
| low_cn0_airliner | Önceki sabit IIR | 0.419 | 0.443 | 0.485 | 100.0 | 99.7 | 0.0 |
|
||||
| low_cn0_airliner | Ham Doppler + ESKF | 0.251 | 0.242 | 1.317 | 100.0 | 100.0 | 0.0 |
|
||||
| low_cn0_airliner | Adaptif IIR + ESKF | 0.206 | 0.203 | 0.338 | 100.0 | 100.0 | 0.0 |
|
||||
| low_cn0_airliner | Ham + robust WLS + ESKF | 0.251 | 0.242 | 1.317 | 100.0 | 100.0 | 0.0 |
|
||||
| single_satellite_outliers | Önceki sabit IIR | 0.495 | 0.478 | 1.146 | 100.0 | 97.7 | 0.0 |
|
||||
| single_satellite_outliers | Ham Doppler + ESKF | 0.257 | 0.229 | 1.451 | 100.0 | 100.0 | 0.0 |
|
||||
| single_satellite_outliers | Adaptif IIR + ESKF | 0.356 | 0.312 | 1.154 | 100.0 | 100.0 | 0.0 |
|
||||
| single_satellite_outliers | Ham + robust WLS + ESKF | 0.191 | 0.179 | 0.418 | 100.0 | 100.0 | 2.9 |
|
||||
| clock_drift_steps | Önceki sabit IIR | 0.416 | 0.438 | 0.440 | 100.0 | 99.7 | 0.0 |
|
||||
| clock_drift_steps | Ham Doppler + ESKF | 0.190 | 0.179 | 0.416 | 100.0 | 100.0 | 0.0 |
|
||||
| clock_drift_steps | Adaptif IIR + ESKF | 0.194 | 0.189 | 0.135 | 100.0 | 100.0 | 0.0 |
|
||||
| clock_drift_steps | Ham + robust WLS + ESKF | 0.190 | 0.179 | 0.416 | 100.0 | 100.0 | 0.0 |
|
||||
| gps_gap_recovery | Önceki sabit IIR | 0.397 | 0.413 | 0.431 | 100.0 | 99.9 | 0.0 |
|
||||
| gps_gap_recovery | Ham Doppler + ESKF | 0.208 | 0.192 | 0.417 | 100.0 | 100.0 | 0.0 |
|
||||
| gps_gap_recovery | Adaptif IIR + ESKF | 0.208 | 0.197 | 0.129 | 100.0 | 100.0 | 0.0 |
|
||||
| gps_gap_recovery | Ham + robust WLS + ESKF | 0.208 | 0.192 | 0.417 | 100.0 | 100.0 | 0.0 |
|
||||
| mixed_flight_faults | Önceki sabit IIR | 0.940 | 0.913 | 1.515 | 100.0 | 60.4 | 0.0 |
|
||||
| mixed_flight_faults | Ham Doppler + ESKF | 0.224 | 0.209 | 1.498 | 100.0 | 100.0 | 0.0 |
|
||||
| mixed_flight_faults | Adaptif IIR + ESKF | 0.295 | 0.275 | 0.838 | 100.0 | 99.9 | 0.0 |
|
||||
| mixed_flight_faults | Ham + robust WLS + ESKF | 0.221 | 0.206 | 1.013 | 100.0 | 100.0 | 1.9 |
|
||||
|
||||
## Bulgular
|
||||
|
||||
- Sabit hızda pre-IIR ESKF RMS'i `0.182` m/s, ham yol `0.190` m/s. Yani pre-IIR sakin durumda daha düzgün çıktı veriyor; ESKF ham gürültüyü zaten `0.416` m/s WLS RMS'ten bu seviyeye indiriyor.
|
||||
- Airliner manevrasında sabit IIR `0.416` m/s ve `%0.0` NIS ret oranı verirken ham yol `0.190` m/s ve `%0.0` ret verdi. Konservatif adapter tabanları rejection'ı sınırlasa bile covariance'ın temsil etmediği IIR gecikmesi fused velocity'de dinamik hata bırakır.
|
||||
- Düşük C/N0 manevrasında sabit/ham/adaptif RMS sırasıyla `0.419`, `0.251`, `0.206` m/s oldu. Adaptif yol bu sentetik koşulda gürültü/gecikme uzlaşmasını sağladı; gerçek eşikler kart verisi ister.
|
||||
- Tek uydu burst'ünde raw WLS RMS `1.451` m/s, robust leave-one-out ile `0.418` m/s. ESKF son RMS farkı daha küçük; çünkü mevcut residual covariance inflation aggregate hatayı zaten zayıflatıyor. Robust kapı yine de hatalı kanalı bütün epoch'u reddetmeden ayırır.
|
||||
- Common clock-drift step raw velocity RMS'ini bozmadı; clock-drift state RMS'i `0.269` m/s. Dördüncü WLS bilinmeyeni ortak frekans hatasını üç hız bileşeninden ayırıyor.
|
||||
|
||||
## Mimari kararı
|
||||
|
||||
- Nominal sentetik model ham Doppler'in sabit pre-IIR gecikmesini önlediğini gösterir; fakat Genesys2 kart-kalibreli noise regresyonunda tam ham yol fazla gürültülüdür. Production ana yolu: hareket-adaptif Doppler -> bandwidth-aware sigma -> kanal validity/outlier kontrolü -> 5 Hz robust velocity WLS + gerçek covariance -> adapter tabanları -> ESKF.
|
||||
- ESKF yalnız 3D aggregate velocity gördüğü için tek uydu hatasını sonradan teşhis edemez. Per-satellite robust gate/FDE WLS öncesinde kalmalıdır.
|
||||
- Ayrı sabit `alpha=0.05` yol PPS drift predictor ve standalone sakin-hız kalite metriği için tutulur; ham snapshot tanısal/offline analiz için korunur.
|
||||
- Production adaptif IIR sakin/hareketli/yüksek-ivme için `alpha=0.05/0.20/0.40` seçer. Alpha büyüdükçe EWMA noise gain'i WLS sigma'sına yansıtılır; böylece FDE ve ESKF covariance bant genişliğini bilir.
|
||||
|
||||
## Model sınırları
|
||||
|
||||
- Sonuçlar sentetik hata büyüklüklerine koşulludur; kartta yüzde kazanç iddiası değildir.
|
||||
- Uydu geometrisi sabit sekiz-kanal local LOS modelidir. Satellite velocity/clock düzeltmelerinin doğru yapıldığı varsayılmıştır.
|
||||
- ESKF, bu soruya etki eden `[velocity, accel-bias]` alt sistemine indirgenmiştir; attitude, lever-arm ve tam position cross-covariance modellenmemiştir.
|
||||
- Tam firmware'deki position update'in velocity/accel-bias cross-covariance üzerinden sağlayabileceği kısmi recovery bu azaltılmış modelde yoktur. Bu nedenle uzun NIS-ret zincirinin mutlak büyüklüğü değil, pre-IIR'in gecikme ve ret mekanizması karar girdisidir.
|
||||
- Carrier loop gürültüsü C/N0 sigma yasasıyla üretilmiştir. Gerçek correlation, oscillator phase noise ve cycle slip dağılımları kart loguyla kalibre edilmelidir.
|
||||
- Robust WLS'nin `3.5 sigma`, en az altı uydu ve bütün tek-kanal leave-one-out seçimi C implementasyonuyla aynıdır. RTL değişmez; raw/filtered ayrımı firmware'dedir.
|
||||
@@ -0,0 +1,33 @@
|
||||
scenario,path,seeds,fused_rms_mps,fused_rms_seed_std_mps,fused_p95_mps,quiet_rms_mps,dynamic_rms_mps,wls_rms_mps,wls_p95_mps,clock_drift_rms_mps,velocity_accept_percent,nis_reject_percent,fused_95_coverage_percent,excluded_epoch_percent,mean_covariance_scale,mean_alpha
|
||||
steady_cruise,fixed_iir_wls_eskf,20,0.1821998934975134,0.040897665127266884,0.27466022199861373,0.1821998934975134,nan,0.0681533332956358,0.11645920599554568,0.04445747832784426,100.0,0.0,100.0,0.0,1.0000030764799477,0.05000000000000001
|
||||
steady_cruise,raw_wls_eskf,20,0.19046432375253602,0.040009781644375395,0.28906917942032884,0.19046432375253602,nan,0.4164164729808367,0.720889200812689,0.268944777665747,100.0,0.0,100.0,0.0,1.2633800541343088,1.0
|
||||
steady_cruise,adaptive_iir_wls_eskf,20,0.1821998934975134,0.040897665127266884,0.27466022199861373,0.1821998934975134,nan,0.0681533332956358,0.11645920599554568,0.04445747832784426,100.0,0.0,100.0,0.0,1.0000030764799477,0.05000000000000001
|
||||
steady_cruise,raw_robust_wls_eskf,20,0.19046432375253602,0.040009781644375395,0.28906917942032884,0.19046432375253602,nan,0.4164164729808367,0.720889200812689,0.268944777665747,100.0,0.0,100.0,0.0,1.2633800541343088,1.0
|
||||
airliner_maneuver,fixed_iir_wls_eskf,20,0.41605194656716626,0.04357555329949249,0.7083141902703941,0.4019739347287984,0.4380205569605863,0.440222331186762,0.9391237648850084,0.04445747832784397,100.0,0.0,99.71411764705883,0.0,1.0000030764799477,0.05000000000000001
|
||||
airliner_maneuver,raw_wls_eskf,20,0.19046432375253933,0.04000978164437598,0.28906917942032373,0.196483054968526,0.1789145728432288,0.4164164729808368,0.720889200812707,0.2689447776657465,100.0,0.0,100.0,0.0,1.2633800541343085,1.0
|
||||
airliner_maneuver,adaptive_iir_wls_eskf,20,0.19421091770965,0.04161706926344872,0.2913238026993562,0.19649429208827177,0.18911500227901482,0.1349904626304289,0.2634780317604405,0.06996150326971154,100.0,0.0,100.0,0.0,1.0000030764799477,0.11647550111358576
|
||||
airliner_maneuver,raw_robust_wls_eskf,20,0.19046432375253933,0.04000978164437598,0.28906917942032373,0.196483054968526,0.1789145728432288,0.4164164729808368,0.720889200812707,0.2689447776657465,100.0,0.0,100.0,0.0,1.2633800541343085,1.0
|
||||
aggressive_maneuver,fixed_iir_wls_eskf,20,0.9420862291590975,0.03697606175587584,1.7661046896298587,0.9551353937753919,0.918769792345089,1.1782538620062029,2.634559496211176,0.04445747832784312,100.0,0.0,55.891176470588235,0.0,1.0000030764799477,0.05000000000000001
|
||||
aggressive_maneuver,raw_wls_eskf,20,0.19046432375254996,0.04000978164436178,0.2890691794203225,0.1928303776618579,0.18561935253806933,0.41641647298083734,0.7208892008127159,0.2689447776657467,100.0,0.0,100.0,0.0,1.2633800541343083,1.0
|
||||
aggressive_maneuver,adaptive_iir_wls_eskf,20,0.20034438129208682,0.04215736847319882,0.3027038479248761,0.2017392380134829,0.19713256311771804,0.17059135831478262,0.3555978539018994,0.08938088837068828,100.0,0.0,100.0,0.0,1.0001185178364316,0.17603563474387524
|
||||
aggressive_maneuver,raw_robust_wls_eskf,20,0.19046432375254996,0.04000978164436178,0.2890691794203225,0.1928303776618579,0.18561935253806933,0.41641647298083734,0.7208892008127159,0.2689447776657467,100.0,0.0,100.0,0.0,1.2633800541343083,1.0
|
||||
low_cn0_airliner,fixed_iir_wls_eskf,20,0.4194988293998554,0.04110688289349549,0.7094271250760412,0.4045729059455276,0.4427791039662977,0.4845544319763686,0.9431345662991619,0.14058689054355467,100.0,0.0,99.74176470588235,0.0,1.0000281042403611,0.05000000000000001
|
||||
low_cn0_airliner,raw_wls_eskf,20,0.25103588282812916,0.04464089418791806,0.4041748199164089,0.2555504221567221,0.2416319579439005,1.3168245098334108,2.2796518151866563,0.8504780622313423,100.0,0.0,100.0,0.0,1.26385941740462,1.0
|
||||
low_cn0_airliner,adaptive_iir_wls_eskf,20,0.20588009483954214,0.040099541607423995,0.3090368268997948,0.2069944088941925,0.20293127741446826,0.337603473203961,0.684354700050294,0.21325106769335284,100.0,0.0,100.0,0.0,1.0000902389090656,0.1040055679287305
|
||||
low_cn0_airliner,raw_robust_wls_eskf,20,0.25103588282812916,0.04464089418791806,0.4041748199164089,0.2555504221567221,0.2416319579439005,1.3168245098334108,2.2796518151866563,0.8504780622313423,100.0,0.0,100.0,0.0,1.26385941740462,1.0
|
||||
single_satellite_outliers,fixed_iir_wls_eskf,20,0.49511238386612133,0.053875439119808514,0.8173709856169348,0.5050054814659805,0.4779542859711989,1.1463378050509494,0.9892652696081077,0.8027251360930692,100.0,0.0,97.6964705882353,0.0,6.927634385063575,0.05000000000000001
|
||||
single_satellite_outliers,raw_wls_eskf,20,0.2570227443883875,0.05273042903908908,0.39439133440568985,0.27216019157834215,0.22927677845507954,1.4512624341492657,0.8297158565959334,1.0630956381619745,100.0,0.0,100.0,0.0,11.13597527632786,1.0
|
||||
single_satellite_outliers,adaptive_iir_wls_eskf,20,0.3556263234893831,0.05851649747663683,0.595609347562141,0.3792437361871613,0.31186327209433895,1.1538474734782178,0.5683617007748845,0.8527337411854743,100.0,0.0,100.0,0.0,7.7254775572955,0.11647550111358576
|
||||
single_satellite_outliers,raw_robust_wls_eskf,20,0.19054941715698687,0.03971061631217381,0.2889429468862187,0.19658211712738688,0.1789788721793858,0.4181969381744313,0.7254268588122093,0.27034697594947305,100.0,0.0,100.0,2.8953229398663693,1.2641999517115465,1.0
|
||||
clock_drift_steps,fixed_iir_wls_eskf,20,0.4160519465671664,0.04357555329949111,0.7083141902703948,0.4019739347287993,0.4380205569605854,0.44022233118676146,0.9391237648850096,1.8177258394243663,100.0,0.0,99.71411764705883,0.0,1.0000030764799477,0.05000000000000001
|
||||
clock_drift_steps,raw_wls_eskf,20,0.19046432375253802,0.04000978164437644,0.28906917942032334,0.19648305496852497,0.17891457284322693,0.41641647298083695,0.7208892008126868,0.268944777665747,100.0,0.0,100.0,0.0,1.2633800541343085,1.0
|
||||
clock_drift_steps,adaptive_iir_wls_eskf,20,0.19421091770965143,0.0416170692634497,0.29132380269935765,0.19649429208827301,0.18911500227901648,0.1349904626304289,0.26347803176043993,1.818528619384018,100.0,0.0,100.0,0.0,1.0000030764799477,0.11647550111358576
|
||||
clock_drift_steps,raw_robust_wls_eskf,20,0.19046432375253802,0.04000978164437644,0.28906917942032334,0.19648305496852497,0.17891457284322693,0.41641647298083695,0.7208892008126868,0.268944777665747,100.0,0.0,100.0,0.0,1.2633800541343085,1.0
|
||||
gps_gap_recovery,fixed_iir_wls_eskf,20,0.3971781427894736,0.044796626065109466,0.7104470646835127,0.3873196668983798,0.41253405912166263,0.43094056066637465,0.9463346669492851,0.04458606585950929,100.0,0.0,99.90529411764706,0.0,1.0000036934211136,0.05
|
||||
gps_gap_recovery,raw_wls_eskf,20,0.2082177514862905,0.03682283889189333,0.32879462613500177,0.21668463381045563,0.1921901394048915,0.41707211010710354,0.7203717161799947,0.26913433965877315,100.0,0.0,100.0,0.0,1.2631217416869522,1.0
|
||||
gps_gap_recovery,adaptive_iir_wls_eskf,20,0.20776527476484566,0.044556420313135676,0.3257106582800945,0.21357805324970008,0.19661285828638408,0.12935782738837362,0.2613749696102362,0.06764578524374312,100.0,0.0,100.0,0.0,1.0000036934211136,0.10814839572192514
|
||||
gps_gap_recovery,raw_robust_wls_eskf,20,0.2082177514862905,0.03682283889189333,0.32879462613500177,0.21668463381045563,0.1921901394048915,0.41707211010710354,0.7203717161799947,0.26913433965877315,100.0,0.0,100.0,0.0,1.2631217416869522,1.0
|
||||
mixed_flight_faults,fixed_iir_wls_eskf,20,0.9404176641021843,0.04319034103529422,1.7661662430364344,0.9555417869207146,0.9132850762857121,1.5154618461064036,2.663221132275041,0.7591896512045151,100.0,0.0,60.37294117647059,0.0,1.3397749809447188,0.05000000000000001
|
||||
mixed_flight_faults,raw_wls_eskf,20,0.22399771088975795,0.05069473595820174,0.3492819306443341,0.23151007332767243,0.2092729705094658,1.498455933396739,2.36689254151483,0.7603191788358898,100.0,0.0,100.0,0.0,2.1864248911994575,1.0
|
||||
mixed_flight_faults,adaptive_iir_wls_eskf,20,0.2952659489493427,0.07200078881833771,0.4874378367724045,0.30541797568187434,0.2754557634719251,0.838415542523151,0.9361678311586857,0.47500979673114047,100.0,0.0,99.92470588235294,0.0,1.250042525522558,0.178938679245283
|
||||
mixed_flight_faults,raw_robust_wls_eskf,20,0.2208132635451996,0.04372541489397352,0.3416439647301807,0.22864253849532457,0.20560052604298312,1.0129266912203836,2.2537697549366906,0.673490930924677,100.0,0.0,100.0,1.9103773584905661,1.5692458289027678,1.0
|
||||
|
@@ -0,0 +1,51 @@
|
||||
scenario,method,epochs,horizontal_rms_m,position_3d_rms_m,position_3d_p95_m,valid_percent,valid_3d_rms_m,horizontal_2sigma_coverage_percent,false_valid_over_25m_percent,fde_percent,mean_measurement_sigma_m
|
||||
clean_open_sky,current_cn0,4500,5.778805688412197,10.054230284262685,17.35250490247279,100.0,10.054230284262685,99.97777777777777,0.3111111111111111,0.0,8.840973580918838
|
||||
clean_open_sky,naive_add_dll,4500,5.768593681325036,10.022431815549082,17.253420427433362,100.0,10.022431815549082,100.0,0.3111111111111111,0.0,10.452052333958017
|
||||
clean_open_sky,dll_excess,4500,5.7784464702133524,10.05299246387535,17.320782018324902,100.0,10.05299246387535,99.97777777777777,0.33333333333333337,0.0,8.908174172560404
|
||||
clean_open_sky,dll_excess_guard,4500,5.7784464702133524,10.05299246387535,17.320782018324902,100.0,10.05299246387535,99.97777777777777,0.33333333333333337,0.0,8.908174172560404
|
||||
clean_open_sky,oracle_variance,4500,5.778805688412197,10.054230284262685,17.35250490247279,100.0,10.054230284262685,99.97777777777777,0.3111111111111111,0.0,8.840973580918838
|
||||
weak_signal,current_cn0,4500,19.020289863331797,32.73415409569256,56.44388754013162,100.0,32.73415409569256,98.8,56.31111111111111,0.6222222222222222,20.84621216112656
|
||||
weak_signal,naive_add_dll,4500,18.802930957724488,32.313115706990146,55.45511304556428,100.0,32.313115706990146,100.0,56.177777777777784,0.0,28.674069502747358
|
||||
weak_signal,dll_excess,4500,18.986226727568837,32.6486650471064,56.115244341908046,100.0,32.6486650471064,98.91111111111111,56.222222222222214,0.5111111111111112,21.20058629982459
|
||||
weak_signal,dll_excess_guard,4500,18.986226727568837,32.6486650471064,56.115244341908046,100.0,32.6486650471064,98.91111111111111,56.222222222222214,0.5111111111111112,21.20058629982459
|
||||
weak_signal,oracle_variance,4500,19.020289863331797,32.73415409569256,56.44388754013162,100.0,32.73415409569256,98.8,56.31111111111111,0.6222222222222222,20.84621216112656
|
||||
one_channel_hidden_jitter,current_cn0,4500,8.804153682379612,12.268224803643907,19.936371518380813,100.0,12.268224803643907,96.46666666666667,1.0666666666666667,9.666666666666666,8.840973580918838
|
||||
one_channel_hidden_jitter,naive_add_dll,4500,6.529225405554112,10.487953223937716,17.74604536639535,100.0,10.487953223937716,99.97777777777777,0.33333333333333337,0.0,11.805515833602682
|
||||
one_channel_hidden_jitter,dll_excess,4500,6.455903980559815,10.46584556632944,17.675128387138177,100.0,10.46584556632944,99.97777777777777,0.33333333333333337,0.044444444444444446,10.358668146326426
|
||||
one_channel_hidden_jitter,dll_excess_guard,4500,6.455903980559815,10.46584556632944,17.675128387138177,100.0,10.46584556632944,99.97777777777777,0.33333333333333337,0.044444444444444446,10.358668146326426
|
||||
one_channel_hidden_jitter,oracle_variance,4500,6.4491935598492525,10.463101509886709,17.736218582311878,100.0,10.463101509886709,99.97777777777777,0.33333333333333337,0.06666666666666667,10.315229331071063
|
||||
common_hidden_jitter,current_cn0,4500,19.733300930595462,33.79270185129192,59.76307034516212,94.75555555555556,33.15386252421736,56.39999999999999,52.400000000000006,46.577777777777776,8.840973580918838
|
||||
common_hidden_jitter,naive_add_dll,4500,16.033969673897577,27.61407554068951,47.55915451805768,100.0,27.61407554068951,99.46666666666667,43.93333333333334,0.26666666666666666,18.882004393424943
|
||||
common_hidden_jitter,dll_excess,4500,16.085615037536463,27.688964211504615,47.828684727805175,100.0,27.688964211504615,98.97777777777777,43.888888888888886,0.4444444444444444,18.04555007574287
|
||||
common_hidden_jitter,dll_excess_guard,4500,16.085615037536463,27.688964211504615,47.828684727805175,100.0,27.688964211504615,98.97777777777777,43.888888888888886,0.4444444444444444,18.04555007574287
|
||||
common_hidden_jitter,oracle_variance,4500,16.106964558267112,27.740128015702773,47.83188793884836,100.0,27.740128015702773,98.88888888888889,43.86666666666667,0.4888888888888889,18.035214487051437
|
||||
noise_burst_recovery,current_cn0,4500,9.325915933961204,12.96069950118485,22.132869786417263,98.93333333333332,12.552632065384056,94.71111111111111,2.577777777777778,12.11111111111111,8.840973580918838
|
||||
noise_burst_recovery,naive_add_dll,4500,6.470839383968352,10.523606281690133,18.031622500344657,100.0,10.523606281690133,100.0,0.5333333333333333,0.06666666666666667,11.60221083527078
|
||||
noise_burst_recovery,dll_excess,4500,6.397124977878571,10.493909835494788,17.99630946143617,100.0,10.493909835494788,99.97777777777777,0.4888888888888889,0.13333333333333333,10.131003114059961
|
||||
noise_burst_recovery,dll_excess_guard,4500,6.397124977878571,10.493909835494788,17.99630946143617,100.0,10.493909835494788,99.97777777777777,0.4888888888888889,0.13333333333333333,10.131003114059961
|
||||
noise_burst_recovery,oracle_variance,4500,6.367173301653832,10.473649717876942,17.90212603773505,100.0,10.473649717876942,99.97777777777777,0.4888888888888889,0.044444444444444446,10.187907003764948
|
||||
dll_transient_stress,current_cn0,4500,5.803776392869777,10.145785618162538,17.425227799952054,100.0,10.145785618162538,99.97777777777777,0.35555555555555557,0.0,8.840973580918838
|
||||
dll_transient_stress,naive_add_dll,4500,5.793995520088368,10.118871049708806,17.333372587049784,100.0,10.118871049708806,100.0,0.37777777777777777,0.0,10.45205253980181
|
||||
dll_transient_stress,dll_excess,4500,5.8034283726190745,10.144627133073621,17.380953993070463,100.0,10.144627133073621,99.97777777777777,0.37777777777777777,0.0,8.908174353046816
|
||||
dll_transient_stress,dll_excess_guard,4500,5.79072382051268,10.123295335548953,17.368844982312577,100.0,10.123295335548953,99.97777777777777,0.33333333333333337,0.0,8.947656011912292
|
||||
dll_transient_stress,oracle_variance,4500,5.803776392869777,10.145785618162538,17.425227799952054,100.0,10.145785618162538,99.97777777777777,0.35555555555555557,0.0,8.840973580918838
|
||||
stable_multipath_bias,current_cn0,4500,7.12694286036133,12.545413964352555,20.99675149186582,100.0,12.545413964352555,99.8888888888889,1.6666666666666667,0.022222222222222223,8.840973580918838
|
||||
stable_multipath_bias,naive_add_dll,4500,7.072680893174009,12.386663925234807,20.866871605887997,100.0,12.386663925234807,99.97777777777777,1.4666666666666666,0.0,10.452052333958017
|
||||
stable_multipath_bias,dll_excess,4500,7.125500558670432,12.536689588412326,21.02122766562977,100.0,12.536689588412326,99.91111111111111,1.6444444444444446,0.022222222222222223,8.908174172560404
|
||||
stable_multipath_bias,dll_excess_guard,4500,7.125500558670432,12.536689588412326,21.02122766562977,100.0,12.536689588412326,99.91111111111111,1.6444444444444446,0.022222222222222223,8.908174172560404
|
||||
stable_multipath_bias,oracle_variance,4500,7.12694286036133,12.545413964352555,20.99675149186582,100.0,12.545413964352555,99.8888888888889,1.6666666666666667,0.022222222222222223,8.840973580918838
|
||||
mixed_multipath,current_cn0,4500,8.529840854093363,14.426936007413373,25.381172069686816,100.0,14.426936007413373,97.11111111111111,5.644444444444444,5.0,8.840973580918838
|
||||
mixed_multipath,naive_add_dll,4500,6.586489015624914,11.258484405711224,19.599950438767102,100.0,11.258484405711224,100.0,0.7333333333333333,0.044444444444444446,11.636563744509052
|
||||
mixed_multipath,dll_excess,4500,6.45479117311332,11.067801578489938,19.233225446062768,100.0,11.067801578489938,100.0,0.7333333333333333,0.08888888888888889,10.202310406784667
|
||||
mixed_multipath,dll_excess_guard,4500,6.45479117311332,11.067801578489938,19.233225446062768,100.0,11.067801578489938,100.0,0.7333333333333333,0.08888888888888889,10.202310406784667
|
||||
mixed_multipath,oracle_variance,4500,6.448333589783235,11.058021985103961,19.19754215965981,100.0,11.058021985103961,99.95555555555555,0.7333333333333333,0.08888888888888889,10.144307488606447
|
||||
poor_geometry_hidden_jitter,current_cn0,4500,79.38795978543642,293.8222926191477,574.9617330272,100.0,293.8222926191477,79.0,94.31111111111112,1.1555555555555554,10.734703317378012
|
||||
poor_geometry_hidden_jitter,naive_add_dll,4500,57.59864532145799,206.13456070122297,405.1527013193,100.0,206.13456070122297,99.66666666666667,92.42222222222222,0.022222222222222223,14.604104871190987
|
||||
poor_geometry_hidden_jitter,dll_excess,4500,55.3927819274298,196.86935630824075,390.63687676393124,100.0,196.86935630824075,99.24444444444444,91.4,0.022222222222222223,12.779670971493758
|
||||
poor_geometry_hidden_jitter,dll_excess_guard,4500,55.3927819274298,196.86935630824075,390.63687676393124,100.0,196.86935630824075,99.24444444444444,91.4,0.022222222222222223,12.779670971493758
|
||||
poor_geometry_hidden_jitter,oracle_variance,4500,55.24662489772763,196.2046647890204,386.5771391089706,100.0,196.2046647890204,99.28888888888889,91.26666666666667,0.022222222222222223,12.721163297137666
|
||||
cn0_step_recovery,current_cn0,4500,12.974359636882712,22.585010269106,47.95054105970103,100.0,22.585010269106,99.42222222222222,21.622222222222224,0.2222222222222222,13.475047639913695
|
||||
cn0_step_recovery,naive_add_dll,4500,12.797089787478507,22.265941453345402,47.59129168788298,100.0,22.265941453345402,100.0,21.422222222222224,0.0,17.44366868773946
|
||||
cn0_step_recovery,dll_excess,4500,12.96657717120691,22.586113735655264,47.91159461508304,100.0,22.586113735655264,99.46666666666667,21.488888888888887,0.2,13.653801199442478
|
||||
cn0_step_recovery,dll_excess_guard,4500,12.96657717120691,22.586113735655264,47.91159461508304,100.0,22.586113735655264,99.46666666666667,21.488888888888887,0.2,13.653801199442478
|
||||
cn0_step_recovery,oracle_variance,4500,12.974359636882712,22.585010269106,47.95054105970103,100.0,22.585010269106,99.42222222222222,21.622222222222224,0.2222222222222222,13.475047639913695
|
||||
|
@@ -0,0 +1,2 @@
|
||||
numpy>=1.24
|
||||
matplotlib>=3.7
|
||||
@@ -0,0 +1,200 @@
|
||||
"""Small executable model of the RTL code-tracking observables.
|
||||
|
||||
This is deliberately scoped to the parts that matter to the navigation
|
||||
measurement study:
|
||||
|
||||
* the normalized early-minus-late magnitude discriminator;
|
||||
* its signed Q2.13 output;
|
||||
* the fixed C1/C2 code-loop filter and Q20 NCO command;
|
||||
* the 20 one-millisecond discriminator samples available per NAV bit.
|
||||
|
||||
It is not an RF waveform simulator. The C/N0-to-discriminator-noise mapping
|
||||
is an explicit calibration assumption until real correlator/DLL telemetry is
|
||||
recorded from the board.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
DLL_OUTPUT_FRACTION_BITS = 13
|
||||
DLL_OUTPUT_WORD_BITS = 15
|
||||
DLL_SPACING_CHIPS = 0.5
|
||||
CODE_LOOP_C1 = 21.25
|
||||
CODE_LOOP_C2 = 0.05712890625
|
||||
CODE_LOOP_OUTPUT_FRACTION_BITS = 20
|
||||
CODE_LOOP_OUTPUT_WORD_BITS = 28
|
||||
DLL_SAMPLES_PER_NAV_BIT = 20
|
||||
CODE_FREQUENCY_BASIS_HZ = 1.023e6
|
||||
ABS_SAMPLE_COUNTER_MODULUS = 1 << 32
|
||||
|
||||
# Synthetic calibration point. It only fixes the scale of the Monte Carlo
|
||||
# discriminator telemetry. Dynamic weighting uses a measured/expected ratio,
|
||||
# so the absolute choice cancels in the correctly modelled case.
|
||||
DLL_STD_AT_45_DBHZ = 0.020
|
||||
|
||||
|
||||
def _signed_saturate(value: int, word_bits: int) -> int:
|
||||
minimum = -(1 << (word_bits - 1))
|
||||
maximum = (1 << (word_bits - 1)) - 1
|
||||
return min(maximum, max(minimum, value))
|
||||
|
||||
|
||||
def quantize_signed(value: float, word_bits: int, fraction_bits: int) -> int:
|
||||
scaled = int(np.rint(value * (1 << fraction_bits)))
|
||||
return _signed_saturate(scaled, word_bits)
|
||||
|
||||
|
||||
def dequantize_signed(value: int, fraction_bits: int) -> float:
|
||||
return float(value) / float(1 << fraction_bits)
|
||||
|
||||
|
||||
def triangular_correlation(code_offset_chips: float) -> float:
|
||||
"""Ideal one-chip triangular GPS C/A autocorrelation main lobe."""
|
||||
|
||||
return max(0.0, 1.0 - abs(code_offset_chips))
|
||||
|
||||
|
||||
def ideal_early_late_magnitudes(
|
||||
code_error_chips: float,
|
||||
spacing_chips: float = DLL_SPACING_CHIPS,
|
||||
) -> tuple[float, float]:
|
||||
half_spacing = 0.5 * spacing_chips
|
||||
early = triangular_correlation(code_error_chips + half_spacing)
|
||||
late = triangular_correlation(code_error_chips - half_spacing)
|
||||
return early, late
|
||||
|
||||
|
||||
def normalized_early_minus_late(early_magnitude: float, late_magnitude: float) -> float:
|
||||
denominator = early_magnitude + late_magnitude
|
||||
if denominator <= 0.0:
|
||||
return 0.0
|
||||
return (early_magnitude - late_magnitude) / denominator
|
||||
|
||||
|
||||
def rtl_discriminator(code_error_chips: float) -> tuple[int, float]:
|
||||
"""Return the RTL-like Q2.13 discriminator code and dequantized value."""
|
||||
|
||||
early, late = ideal_early_late_magnitudes(code_error_chips)
|
||||
value = normalized_early_minus_late(early, late)
|
||||
code = quantize_signed(
|
||||
value,
|
||||
DLL_OUTPUT_WORD_BITS,
|
||||
DLL_OUTPUT_FRACTION_BITS,
|
||||
)
|
||||
return code, dequantize_signed(code, DLL_OUTPUT_FRACTION_BITS)
|
||||
|
||||
|
||||
def expected_discriminator_std(cn0_dbhz: float) -> float:
|
||||
"""Expected normalized DLL jitter used by the synthetic calibration.
|
||||
|
||||
The 20 dB amplitude-domain law mirrors the firmware's code-sigma model.
|
||||
The result is bounded because the normalized discriminator is finite.
|
||||
"""
|
||||
|
||||
value = DLL_STD_AT_45_DBHZ * 10.0 ** ((45.0 - cn0_dbhz) / 20.0)
|
||||
return float(np.clip(value, 0.002, 0.35))
|
||||
|
||||
|
||||
def quantized_code_epoch_measurement(
|
||||
true_abs_sample: float,
|
||||
) -> tuple[int, int, float]:
|
||||
"""Model ABS_SAMPLE + REM_CODE_PHASE and firmware reconstruction.
|
||||
|
||||
``true_abs_sample`` is the ideal code boundary in RF-sample units. The
|
||||
hardware captures the following integer sample and reports the overshoot
|
||||
with the remainder scale used by the C correction formula.
|
||||
"""
|
||||
|
||||
raw_unwrapped = int(np.ceil(true_abs_sample))
|
||||
overshoot_samples = raw_unwrapped - true_abs_sample
|
||||
remainder = int(np.rint(overshoot_samples * CODE_FREQUENCY_BASIS_HZ))
|
||||
raw_register = raw_unwrapped % ABS_SAMPLE_COUNTER_MODULUS
|
||||
corrected = raw_unwrapped - remainder / CODE_FREQUENCY_BASIS_HZ
|
||||
return raw_register, remainder, corrected
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DllWindow:
|
||||
samples: np.ndarray
|
||||
mean: float
|
||||
variance: float
|
||||
expected_variance: float
|
||||
variance_ratio: float
|
||||
mean_z_score: float
|
||||
|
||||
|
||||
def synthesize_dll_window(
|
||||
rng: np.random.Generator,
|
||||
*,
|
||||
cn0_dbhz: float,
|
||||
jitter_multiplier: float = 1.0,
|
||||
discriminator_bias: float = 0.0,
|
||||
sample_count: int = DLL_SAMPLES_PER_NAV_BIT,
|
||||
) -> DllWindow:
|
||||
"""Generate and Q2.13-quantize one NAV-bit DLL telemetry window."""
|
||||
|
||||
if sample_count < 2:
|
||||
raise ValueError("DLL variance needs at least two samples")
|
||||
expected_std = expected_discriminator_std(cn0_dbhz)
|
||||
actual_std = expected_std * max(0.0, jitter_multiplier)
|
||||
raw = discriminator_bias + rng.normal(0.0, actual_std, sample_count)
|
||||
minimum = -(1 << (DLL_OUTPUT_WORD_BITS - 1))
|
||||
maximum = (1 << (DLL_OUTPUT_WORD_BITS - 1)) - 1
|
||||
quantized_codes = np.clip(
|
||||
np.rint(raw * (1 << DLL_OUTPUT_FRACTION_BITS)),
|
||||
minimum,
|
||||
maximum,
|
||||
).astype(np.int32)
|
||||
samples = quantized_codes.astype(float) / float(1 << DLL_OUTPUT_FRACTION_BITS)
|
||||
mean = float(np.mean(samples))
|
||||
variance = float(np.var(samples, ddof=1))
|
||||
expected_variance = expected_std * expected_std
|
||||
ratio = variance / max(expected_variance, 1.0e-15)
|
||||
standard_error = np.sqrt(max(variance, expected_variance) / sample_count)
|
||||
mean_z = abs(mean) / max(float(standard_error), 1.0e-12)
|
||||
return DllWindow(
|
||||
samples=samples,
|
||||
mean=mean,
|
||||
variance=variance,
|
||||
expected_variance=expected_variance,
|
||||
variance_ratio=float(ratio),
|
||||
mean_z_score=float(mean_z),
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class CodeLoopFilterRtl:
|
||||
"""Update-rate model of LF_code.sv, including coefficient quantization."""
|
||||
|
||||
integrator_q20: int = 0
|
||||
|
||||
c1_integer: int = int(CODE_LOOP_C1 * (1 << 2))
|
||||
c2_integer: int = int(CODE_LOOP_C2 * (1 << 11))
|
||||
|
||||
def update_code(self, discriminator_q13: int) -> int:
|
||||
# C1 product has Q15 and is zero-padded to Q20 in LF_code.sv.
|
||||
proportional_q20 = (self.c1_integer * discriminator_q13) << 5
|
||||
|
||||
# C2 product has Q24 and LF_code.sv slices it back to Q20.
|
||||
integral_increment_q20 = (self.c2_integer * discriminator_q13) >> 4
|
||||
self.integrator_q20 = _signed_saturate(
|
||||
self.integrator_q20 + integral_increment_q20,
|
||||
CODE_LOOP_OUTPUT_WORD_BITS,
|
||||
)
|
||||
return _signed_saturate(
|
||||
self.integrator_q20 + proportional_q20,
|
||||
CODE_LOOP_OUTPUT_WORD_BITS,
|
||||
)
|
||||
|
||||
def update(self, discriminator: float) -> float:
|
||||
discriminator_q13 = quantize_signed(
|
||||
discriminator,
|
||||
DLL_OUTPUT_WORD_BITS,
|
||||
DLL_OUTPUT_FRACTION_BITS,
|
||||
)
|
||||
command_q20 = self.update_code(discriminator_q13)
|
||||
return dequantize_signed(command_q20, CODE_LOOP_OUTPUT_FRACTION_BITS)
|
||||
@@ -0,0 +1,212 @@
|
||||
"""Regression tests for the project-specific Doppler/WLS/ESKF model."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import replace
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .doppler_eskf_model import (
|
||||
AdaptiveAlpha,
|
||||
DopplerPath,
|
||||
FIXED_DOPPLER_ALPHA,
|
||||
flight_scenarios,
|
||||
local_los_matrix,
|
||||
run_doppler_eskf_scenario,
|
||||
velocity_wls,
|
||||
)
|
||||
|
||||
|
||||
def _scenario(name: str):
|
||||
return next(item for item in flight_scenarios() if item.name == name)
|
||||
|
||||
|
||||
def _fused_rms(trace) -> float:
|
||||
selected = trace.time_s >= 5.0
|
||||
error = trace.fused_velocity_mps[selected] - trace.truth_velocity_mps[selected]
|
||||
return float(np.sqrt(np.mean(np.sum(np.square(error), axis=1))))
|
||||
|
||||
|
||||
def _wls_rms(trace) -> float:
|
||||
truth = np.column_stack(
|
||||
[
|
||||
np.interp(
|
||||
trace.wls_time_s,
|
||||
trace.time_s,
|
||||
trace.truth_velocity_mps[:, axis],
|
||||
)
|
||||
for axis in range(3)
|
||||
]
|
||||
)
|
||||
error = trace.wls_velocity_mps - truth
|
||||
return float(np.sqrt(np.mean(np.sum(np.square(error), axis=1))))
|
||||
|
||||
|
||||
def _board_raw_noise_scenario(name: str):
|
||||
"""Match the 9.17 m/s raw-WLS RMS measured on Genesys2."""
|
||||
scenario = _scenario(name)
|
||||
nominal_noise = scenario.noise_multiplier
|
||||
return replace(
|
||||
scenario,
|
||||
name=f"{scenario.name}_genesys2_noise",
|
||||
noise_multiplier=lambda time_s, satellite: (
|
||||
22.0 * nominal_noise(time_s, satellite)
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
class DopplerWlsTest(unittest.TestCase):
|
||||
def test_four_state_wls_separates_velocity_and_clock_drift(self) -> None:
|
||||
velocity = np.asarray([145.0, -32.0, 7.5])
|
||||
clock_drift = 18.0
|
||||
observation = -local_los_matrix() @ velocity + clock_drift
|
||||
result = velocity_wls(observation, np.full(8, 45.0))
|
||||
|
||||
np.testing.assert_allclose(result.velocity_mps, velocity, atol=1.0e-10)
|
||||
self.assertAlmostEqual(result.clock_drift_mps, clock_drift, places=10)
|
||||
self.assertAlmostEqual(result.covariance_scale, 1.0)
|
||||
|
||||
def test_robust_wls_excludes_a_single_bad_channel(self) -> None:
|
||||
velocity = np.asarray([120.0, 20.0, -2.0])
|
||||
observation = -local_los_matrix() @ velocity + 4.0
|
||||
observation[2] += 12.0
|
||||
|
||||
plain = velocity_wls(observation, np.full(8, 43.0))
|
||||
robust = velocity_wls(observation, np.full(8, 43.0), robust=True)
|
||||
|
||||
self.assertEqual(robust.excluded_satellite, 2)
|
||||
self.assertGreater(np.linalg.norm(plain.velocity_mps - velocity), 1.0)
|
||||
np.testing.assert_allclose(robust.velocity_mps, velocity, atol=1.0e-9)
|
||||
|
||||
|
||||
class AdaptiveAlphaTest(unittest.TestCase):
|
||||
def test_motion_switches_alpha_and_quiet_state_recovers(self) -> None:
|
||||
selector = AdaptiveAlpha()
|
||||
for _ in range(80):
|
||||
quiet = selector.update(np.zeros(3), 35.0)
|
||||
self.assertAlmostEqual(quiet, 0.03)
|
||||
|
||||
dynamic_values = [
|
||||
selector.update(np.asarray([4.0, 0.0, 0.0]), 35.0)
|
||||
for _ in range(30)
|
||||
]
|
||||
self.assertGreaterEqual(max(dynamic_values), 0.40)
|
||||
|
||||
for _ in range(120):
|
||||
recovered = selector.update(np.zeros(3), 40.0)
|
||||
self.assertAlmostEqual(recovered, FIXED_DOPPLER_ALPHA)
|
||||
|
||||
|
||||
class DopplerEskfIntegrationTest(unittest.TestCase):
|
||||
def test_board_raw_noise_requires_adaptive_prefilter(self) -> None:
|
||||
steady = run_doppler_eskf_scenario(
|
||||
_board_raw_noise_scenario("steady_cruise"),
|
||||
seed=7,
|
||||
paths=(
|
||||
DopplerPath.FIXED_IIR,
|
||||
DopplerPath.ADAPTIVE_IIR,
|
||||
DopplerPath.RAW,
|
||||
),
|
||||
)
|
||||
airliner = run_doppler_eskf_scenario(
|
||||
_board_raw_noise_scenario("airliner_maneuver"),
|
||||
seed=3,
|
||||
paths=(
|
||||
DopplerPath.FIXED_IIR,
|
||||
DopplerPath.ADAPTIVE_IIR,
|
||||
DopplerPath.RAW,
|
||||
),
|
||||
)
|
||||
aggressive = run_doppler_eskf_scenario(
|
||||
_board_raw_noise_scenario("aggressive_maneuver"),
|
||||
seed=7,
|
||||
paths=(
|
||||
DopplerPath.FIXED_IIR,
|
||||
DopplerPath.ADAPTIVE_IIR,
|
||||
DopplerPath.RAW,
|
||||
),
|
||||
)
|
||||
steady_fixed = _fused_rms(steady[DopplerPath.FIXED_IIR])
|
||||
steady_adaptive = _fused_rms(steady[DopplerPath.ADAPTIVE_IIR])
|
||||
steady_raw = _fused_rms(steady[DopplerPath.RAW])
|
||||
airliner_fixed = _fused_rms(airliner[DopplerPath.FIXED_IIR])
|
||||
airliner_adaptive = _fused_rms(airliner[DopplerPath.ADAPTIVE_IIR])
|
||||
airliner_raw = _fused_rms(airliner[DopplerPath.RAW])
|
||||
aggressive_fixed = _fused_rms(aggressive[DopplerPath.FIXED_IIR])
|
||||
aggressive_adaptive = _fused_rms(
|
||||
aggressive[DopplerPath.ADAPTIVE_IIR]
|
||||
)
|
||||
aggressive_raw = _fused_rms(aggressive[DopplerPath.RAW])
|
||||
|
||||
# Direct 20 ms snapshots are too noisy at the level actually observed
|
||||
# on the board. Adaptive bandwidth retains the quiet-state result and
|
||||
# beats both direct raw and the 0.39 s fixed-IIR lag in manoeuvres.
|
||||
self.assertLess(steady_adaptive, 1.05 * steady_fixed)
|
||||
self.assertLess(steady_adaptive, 0.50 * steady_raw)
|
||||
self.assertLess(airliner_adaptive, airliner_fixed)
|
||||
self.assertLess(airliner_adaptive, 0.50 * airliner_raw)
|
||||
self.assertLess(aggressive_adaptive, 0.65 * aggressive_fixed)
|
||||
self.assertLess(aggressive_adaptive, 0.50 * aggressive_raw)
|
||||
|
||||
def test_eskf_filters_raw_doppler_without_pre_iir(self) -> None:
|
||||
traces = run_doppler_eskf_scenario(
|
||||
_scenario("steady_cruise"),
|
||||
seed=7,
|
||||
paths=(DopplerPath.RAW,),
|
||||
)
|
||||
raw = traces[DopplerPath.RAW]
|
||||
self.assertLess(_fused_rms(raw), 0.45 * _wls_rms(raw))
|
||||
self.assertGreater(float(np.mean(raw.wls_accepted)), 0.99)
|
||||
|
||||
def test_raw_path_avoids_fixed_iir_manoeuvre_lag(self) -> None:
|
||||
traces = run_doppler_eskf_scenario(
|
||||
_scenario("airliner_maneuver"),
|
||||
seed=3,
|
||||
paths=(DopplerPath.FIXED_IIR, DopplerPath.RAW),
|
||||
)
|
||||
fixed = traces[DopplerPath.FIXED_IIR]
|
||||
raw = traces[DopplerPath.RAW]
|
||||
self.assertLess(_fused_rms(raw), 0.35 * _fused_rms(fixed))
|
||||
# The production 2/2/3 m/s adapter floors keep both paths inside the
|
||||
# NIS gate; the fixed-IIR penalty now appears as lag error, not reject.
|
||||
self.assertGreater(float(np.mean(raw.wls_accepted)), 0.99)
|
||||
self.assertGreater(float(np.mean(fixed.wls_accepted)), 0.99)
|
||||
|
||||
def test_common_clock_step_does_not_leak_into_raw_velocity(self) -> None:
|
||||
airliner = run_doppler_eskf_scenario(
|
||||
_scenario("airliner_maneuver"),
|
||||
seed=11,
|
||||
paths=(DopplerPath.RAW,),
|
||||
)[DopplerPath.RAW]
|
||||
clock_step = run_doppler_eskf_scenario(
|
||||
_scenario("clock_drift_steps"),
|
||||
seed=11,
|
||||
paths=(DopplerPath.RAW,),
|
||||
)[DopplerPath.RAW]
|
||||
|
||||
np.testing.assert_allclose(
|
||||
airliner.wls_velocity_mps,
|
||||
clock_step.wls_velocity_mps,
|
||||
atol=1.0e-10,
|
||||
)
|
||||
np.testing.assert_allclose(
|
||||
airliner.fused_velocity_mps,
|
||||
clock_step.fused_velocity_mps,
|
||||
atol=1.0e-10,
|
||||
)
|
||||
|
||||
def test_robust_wls_contains_channel_bursts_before_eskf(self) -> None:
|
||||
traces = run_doppler_eskf_scenario(
|
||||
_scenario("single_satellite_outliers"),
|
||||
seed=5,
|
||||
paths=(DopplerPath.RAW, DopplerPath.RAW_ROBUST),
|
||||
)
|
||||
raw = traces[DopplerPath.RAW]
|
||||
robust = traces[DopplerPath.RAW_ROBUST]
|
||||
self.assertLess(_wls_rms(robust), 0.65 * _wls_rms(raw))
|
||||
self.assertGreater(float(np.mean(robust.excluded_satellites >= 0)), 0.02)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,182 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
from gps_measurement_python.code_smoothing_model import run_smoothing_trial
|
||||
from gps_measurement_python.firmware_wls_model import (
|
||||
SigmaInputs,
|
||||
SigmaMethod,
|
||||
firmware_sigma_m,
|
||||
)
|
||||
from gps_measurement_python.measurement_scenarios import (
|
||||
run_scenario,
|
||||
scenario_matrix,
|
||||
)
|
||||
from gps_measurement_python.rtl_dll_model import (
|
||||
CodeLoopFilterRtl,
|
||||
quantized_code_epoch_measurement,
|
||||
rtl_discriminator,
|
||||
synthesize_dll_window,
|
||||
)
|
||||
|
||||
|
||||
class RtlDllModelTests(unittest.TestCase):
|
||||
def test_discriminator_zero_and_q13_symmetry(self):
|
||||
self.assertEqual(rtl_discriminator(0.0)[0], 0)
|
||||
positive = rtl_discriminator(0.05)[1]
|
||||
negative = rtl_discriminator(-0.05)[1]
|
||||
self.assertAlmostEqual(positive, -negative, places=6)
|
||||
self.assertLess(positive, 0.0)
|
||||
|
||||
def test_code_loop_integrator_accumulates_constant_error(self):
|
||||
loop = CodeLoopFilterRtl()
|
||||
first = loop.update(0.01)
|
||||
twentieth = first
|
||||
for _ in range(19):
|
||||
twentieth = loop.update(0.01)
|
||||
self.assertGreater(first, 0.0)
|
||||
self.assertGreater(twentieth, first)
|
||||
|
||||
def test_dll_window_detects_excess_variance(self):
|
||||
rng = np.random.default_rng(42)
|
||||
nominal = synthesize_dll_window(
|
||||
rng,
|
||||
cn0_dbhz=45.0,
|
||||
jitter_multiplier=1.0,
|
||||
sample_count=4000,
|
||||
)
|
||||
noisy = synthesize_dll_window(
|
||||
rng,
|
||||
cn0_dbhz=45.0,
|
||||
jitter_multiplier=3.0,
|
||||
sample_count=4000,
|
||||
)
|
||||
self.assertAlmostEqual(nominal.variance_ratio, 1.0, delta=0.10)
|
||||
self.assertAlmostEqual(noisy.variance_ratio, 9.0, delta=0.70)
|
||||
|
||||
def test_abs_sample_remainder_reconstructs_subsample_epoch(self):
|
||||
for true_sample in (
|
||||
12_345_678.314159,
|
||||
12_345_678.0000001,
|
||||
12_345_678.9999999,
|
||||
12_345_678.0,
|
||||
):
|
||||
raw, remainder, corrected = quantized_code_epoch_measurement(true_sample)
|
||||
self.assertEqual(raw, int(np.ceil(true_sample)))
|
||||
self.assertLessEqual(
|
||||
abs(corrected - true_sample),
|
||||
0.5 / 1.023e6 + 1.0e-12,
|
||||
)
|
||||
self.assertGreaterEqual(remainder, 0)
|
||||
|
||||
|
||||
class FirmwareSigmaTests(unittest.TestCase):
|
||||
@staticmethod
|
||||
def inputs(ratio: float = 1.0, mean_z: float = 0.0) -> SigmaInputs:
|
||||
return SigmaInputs(
|
||||
ura_index=np.asarray([0]),
|
||||
elevation_deg=np.asarray([90.0]),
|
||||
cn0_dbhz=np.asarray([45.0]),
|
||||
dll_variance_ratio=np.asarray([ratio]),
|
||||
dll_mean_z=np.asarray([mean_z]),
|
||||
actual_jitter_multiplier=np.asarray([np.sqrt(ratio)]),
|
||||
)
|
||||
|
||||
def test_current_matches_c_formula(self):
|
||||
sigma = firmware_sigma_m(self.inputs(), SigmaMethod.CURRENT)[0]
|
||||
expected = np.sqrt(2.4**2 + 3.0**2 + 3.0**2)
|
||||
self.assertAlmostEqual(sigma, expected, places=12)
|
||||
|
||||
def test_excess_method_does_not_double_count_nominal_dll_noise(self):
|
||||
current = firmware_sigma_m(self.inputs(), SigmaMethod.CURRENT)[0]
|
||||
excess = firmware_sigma_m(self.inputs(), SigmaMethod.DLL_EXCESS)[0]
|
||||
naive = firmware_sigma_m(self.inputs(), SigmaMethod.NAIVE_ADD_DLL)[0]
|
||||
self.assertAlmostEqual(excess, current, places=12)
|
||||
self.assertGreater(naive, current)
|
||||
|
||||
def test_excess_method_inflates_hidden_jitter(self):
|
||||
current = firmware_sigma_m(self.inputs(9.0), SigmaMethod.CURRENT)[0]
|
||||
excess = firmware_sigma_m(self.inputs(9.0), SigmaMethod.DLL_EXCESS)[0]
|
||||
self.assertGreater(excess, 1.8 * current)
|
||||
|
||||
|
||||
class ScenarioTests(unittest.TestCase):
|
||||
def test_hidden_jitter_dynamic_sigma_improves_seeded_case(self):
|
||||
scenario = next(
|
||||
item
|
||||
for item in scenario_matrix()
|
||||
if item.name == "one_channel_hidden_jitter"
|
||||
)
|
||||
samples = run_scenario(
|
||||
scenario,
|
||||
17,
|
||||
(SigmaMethod.CURRENT, SigmaMethod.DLL_EXCESS),
|
||||
)
|
||||
current = np.asarray(
|
||||
[
|
||||
item.position_error_3d_m
|
||||
for item in samples
|
||||
if item.method == SigmaMethod.CURRENT and item.time_s >= 5.0
|
||||
]
|
||||
)
|
||||
dynamic = np.asarray(
|
||||
[
|
||||
item.position_error_3d_m
|
||||
for item in samples
|
||||
if item.method == SigmaMethod.DLL_EXCESS and item.time_s >= 5.0
|
||||
]
|
||||
)
|
||||
self.assertLess(
|
||||
np.sqrt(np.mean(dynamic * dynamic)),
|
||||
0.90 * np.sqrt(np.mean(current * current)),
|
||||
)
|
||||
|
||||
def test_stable_multipath_is_not_solved_by_variance_only(self):
|
||||
scenario = next(
|
||||
item
|
||||
for item in scenario_matrix()
|
||||
if item.name == "stable_multipath_bias"
|
||||
)
|
||||
samples = run_scenario(
|
||||
scenario,
|
||||
23,
|
||||
(SigmaMethod.CURRENT, SigmaMethod.DLL_EXCESS),
|
||||
)
|
||||
current = np.asarray(
|
||||
[
|
||||
item.position_error_3d_m
|
||||
for item in samples
|
||||
if item.method == SigmaMethod.CURRENT and item.time_s >= 5.0
|
||||
]
|
||||
)
|
||||
dynamic = np.asarray(
|
||||
[
|
||||
item.position_error_3d_m
|
||||
for item in samples
|
||||
if item.method == SigmaMethod.DLL_EXCESS and item.time_s >= 5.0
|
||||
]
|
||||
)
|
||||
relative_difference = abs(
|
||||
np.sqrt(np.mean(dynamic * dynamic))
|
||||
/ np.sqrt(np.mean(current * current))
|
||||
- 1.0
|
||||
)
|
||||
self.assertLess(relative_difference, 0.08)
|
||||
|
||||
def test_doppler_prediction_avoids_blind_iir_dynamic_lag(self):
|
||||
result = run_smoothing_trial(31, doppler_cfo_mps=0.0)
|
||||
self.assertLess(result.doppler_smoothed_rmse_m, result.raw_rmse_m)
|
||||
self.assertLess(
|
||||
result.doppler_peak_dynamic_error_m,
|
||||
0.10 * result.scalar_iir_peak_dynamic_error_m,
|
||||
)
|
||||
self.assertGreater(
|
||||
result.firmware_iir_doppler_peak_dynamic_error_m,
|
||||
result.doppler_peak_dynamic_error_m,
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -82,6 +82,8 @@ BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c
|
||||
|
||||
GPS_NAV_SOLVE_TIMER_TICKS ?= 10
|
||||
GPS_PPS_STATUS_LOG_INTERVAL ?= 10
|
||||
GPS_PPS_DOPPLER_PREDICTOR_ENABLE ?= 1
|
||||
GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES ?= 0
|
||||
|
||||
include ../common/app.mk
|
||||
|
||||
@@ -94,7 +96,9 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \
|
||||
-DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \
|
||||
-DIMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=$(IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES) \
|
||||
-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS) \
|
||||
-DGPS_PPS_STATUS_LOG_INTERVAL=$(GPS_PPS_STATUS_LOG_INTERVAL)
|
||||
-DGPS_PPS_STATUS_LOG_INTERVAL=$(GPS_PPS_STATUS_LOG_INTERVAL) \
|
||||
-DGPS_PPS_DOPPLER_PREDICTOR_ENABLE=$(GPS_PPS_DOPPLER_PREDICTOR_ENABLE) \
|
||||
-DGPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=$(GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES)
|
||||
LDLIBS += -lm
|
||||
|
||||
ifeq ($(GPS_DEBUG),1)
|
||||
|
||||
@@ -1,6 +1,8 @@
|
||||
# GPS System Port Notes
|
||||
|
||||
Bu notlar GPS bare-metal yaziliminin VCU118 Rocket SoC portu ve interrupt debug sureci icin yazildi.
|
||||
Bu notlar GPS bare-metal yaziliminin VCU118/Genesys2 Rocket SoC portunu,
|
||||
tracking Doppler/C/N0 yolunu, ESKF entegrasyonunu ve interrupt debug surecini
|
||||
belgeler.
|
||||
|
||||
## Uygulama akisi ve dosya sinirlari
|
||||
|
||||
@@ -66,6 +68,34 @@ ve raw kuyruga koyar. GPS ISR'leri acquisition/tracking olaylarini kaydeder.
|
||||
interrupt'lari gelebilecegi icin IMU kuyrugu WLS'den hem once hem sonra drain
|
||||
edilir; GPS update'i WLS bitis zamaninda degil capture epoch'unda uygulanir.
|
||||
|
||||
### ESKF entegrasyonunda yapilanlar
|
||||
|
||||
- TDK ICM42688 ve ST LSM6DSOX suruculeri sensor-markasindan bagimsiz
|
||||
`imu_sensor`/`imu_sample_t` sinirinin arkasina alindi. Her IMU ornegi olcum
|
||||
aninda monotonik CLINT timestamp'i alir; ISR icinde ESKF matematigi
|
||||
calismaz.
|
||||
- Filtre ENU position/velocity, quaternion attitude, accel bias ve gyro bias
|
||||
iceren 15 hata durumlu ESKF'dir. Prediction her ham IMU orneginde midpoint
|
||||
mechanization ile calisir; telemetry bolucusu prediction oranini dusurmez.
|
||||
- GPS WLS position ve Doppler velocity ayni capture timestamp'iyle tek
|
||||
`fusion_navigation_measurement_t` icinde tasinir. Gecikmis GPS olcumu
|
||||
history icindeki kendi epoch'una uygulanir; sonraki IMU ornekleri current
|
||||
time'a kadar tekrar oynatilir.
|
||||
- Position ve velocity update'lerinin ayri 3-DOF NIS kapilari ve
|
||||
accept/reject sayaclari vardir. Velocity bozuksa ayni epoch'taki gecerli
|
||||
position update'i geri alinmaz.
|
||||
- Doppler WLS ECEF velocity covariance'i ENU'ya dondurulur. Kart verisinde
|
||||
zayif geometrinin formal covariance'i asiri iyimser yapmamasi icin
|
||||
horizontal `2.0 m/s`, vertical `3.0 m/s` minimum standard deviation floor'u
|
||||
uygulanir; daha buyuk formal belirsizlik korunur.
|
||||
- Durağanlik supervisor'u IMU/GNSS hareket kanitlarini histerezisle
|
||||
birlestirir ve yalniz guvenli durumda ZUPT/zero-gyro update uygular.
|
||||
GPS kesintisinde sahte position olcumu uretilmez; dead-reckoning devam eder
|
||||
ve covariance buyur.
|
||||
- Kotu ilk fix sonraki tutarli fix'leri surekli NIS reject'e kilitlerse,
|
||||
timeout ve coklu-fix sureklilik kosuluyla yalniz position state/covariance
|
||||
yeniden ankrajlanir. Velocity, attitude ve IMU bias state'leri korunur.
|
||||
|
||||
## Generic IMU Mimarisi (TDK ve ST)
|
||||
|
||||
GPS/ESKF artik dogrudan bir sensor surucusune bagli degildir. Veri yolu:
|
||||
@@ -129,6 +159,41 @@ Bu durumda GPS acquisition/tracking/WLS ve ham lat-lon yayini calismaya devam
|
||||
eder; IMU stream'i ve `navigation_fusion` baslatilmaz. UART startup satirinda
|
||||
`eskf_enabled` ve gercek init sonucu olan `eskf_ready` ayri alanlardir.
|
||||
|
||||
Ana proje dizininden Genesys2 MCS uretmek icin:
|
||||
|
||||
```sh
|
||||
# TDK + ESKF; GPS_ESKF_ENABLE verilmezse de varsayilan 1'dir.
|
||||
make FPGA_BOARD=genesys2 APP=gps_system \
|
||||
GPS_ESKF_ENABLE=1 IMU_SENSOR=tdk fpga-mcs
|
||||
|
||||
# GPS-only: tracking, C/N0, Doppler velocity WLS ve PPS calisir.
|
||||
make FPGA_BOARD=genesys2 APP=gps_system \
|
||||
GPS_ESKF_ENABLE=0 fpga-mcs
|
||||
```
|
||||
|
||||
Her iki profil bootloader ve uygulamayi varsayilan `460800` baud ile derler.
|
||||
Iki komut ayni MCS/BIN yoluna yazdigi icin iki imaj birlikte saklanacaksa
|
||||
build'ler arasinda profil adiyla kopyalanmalidir. Bu secenekler software
|
||||
derleme parametresidir; mevcut bitstream ile `fpga-mcs` yeterlidir.
|
||||
|
||||
PPS Doppler datum-drift predictor production'da varsayilan olarak aciktir.
|
||||
Kontrollu A/B icin build-time secenekleri:
|
||||
|
||||
```sh
|
||||
# Production: predictor sürekli acik, otomatik A/B yok.
|
||||
make -C soc/software/gps_system \
|
||||
GPS_PPS_DOPPLER_PREDICTOR_ENABLE=1 \
|
||||
GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=0
|
||||
|
||||
# Tani: her 200 navigation update'inda (~40 s) kapali/acik degistir.
|
||||
make -C soc/software/gps_system \
|
||||
GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=200
|
||||
```
|
||||
|
||||
`GPS_PPS_DOPPLER_PREDICTOR_ENABLE=0` yalniz kontrollu fallback testi icindir.
|
||||
`GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES` sifirdan buyukse A/B secimi enable
|
||||
degerini bilerek ezer; production image'da mutlaka `0` kalmalidir.
|
||||
|
||||
Otomatik WHO_AM_I algilama varsayilandir:
|
||||
|
||||
```sh
|
||||
@@ -446,32 +511,30 @@ Bu kosuda `mcause=5` tekrar etmedi.
|
||||
|
||||
## Build ve Debug Komutlari
|
||||
|
||||
Build:
|
||||
Ana dizinden kalici flash imaji:
|
||||
|
||||
```sh
|
||||
make -C /home/ankaoc/Desktop/rocket/rocket-chip/soc/software/gps_system \
|
||||
BUILD_DIR=/home/ankaoc/Desktop/rocket/rocket-chip/soc/out/software/gps_system \
|
||||
clean all
|
||||
make FPGA_BOARD=genesys2 APP=gps_system \
|
||||
GPS_ESKF_ENABLE=1 IMU_SENSOR=tdk fpga-mcs
|
||||
```
|
||||
|
||||
OpenOCD:
|
||||
|
||||
```sh
|
||||
make -C /home/ankaoc/Desktop/rocket/rocket-chip fpga-debug-openocd
|
||||
make FPGA_BOARD=genesys2 fpga-debug-openocd
|
||||
```
|
||||
|
||||
ELF'i RAM'e yukleyip calistirma:
|
||||
|
||||
```sh
|
||||
/tools/toolchains/rvtc-medany/bin/riscv64-unknown-elf-gdb \
|
||||
-q /home/ankaoc/Desktop/rocket/rocket-chip/soc/out/software/gps_system/gps_system.elf \
|
||||
-ex "target extended-remote :3333" \
|
||||
-ex "monitor reset halt" \
|
||||
-ex "load" \
|
||||
-ex "set \$pc = _start" \
|
||||
-ex "monitor resume" \
|
||||
-ex "detach" \
|
||||
-ex "quit"
|
||||
make FPGA_BOARD=genesys2 APP=gps_system \
|
||||
GPS_ESKF_ENABLE=1 IMU_SENSOR=tdk fpga-debug-load
|
||||
```
|
||||
|
||||
UART:
|
||||
|
||||
```sh
|
||||
make FPGA_BOARD=genesys2 fpga-uart
|
||||
```
|
||||
|
||||
Durum okumak icin claim register'ini okumadan:
|
||||
@@ -749,6 +812,13 @@ Gecersiz oranlar reddedilir; sonuc `10..60 dB-Hz` araliginda sinirlanir.
|
||||
Bir-bit estimator jitter'i ayni PRN'in ard arda olcumlerinde software
|
||||
IIR'i ile `alpha=0.2` kullanilarak yumusatilir.
|
||||
|
||||
RTL snapshot, synchronizer, APB register ve measurement-interrupt ayrintilari
|
||||
`hardware/external/gps/rtl/trck_src/README.md` icinde belgelenir. Ozetle
|
||||
payload tracking domaininde yaklasik 20 ms sabit tutulur; valid ve tum alanlar
|
||||
PCLK'e registered synchronizer ile alinir, iki ek settle cycle'dan sonra
|
||||
atomik APB bankina kopyalanir. Software sequence-before/payload/sequence-after
|
||||
okumasiyla coherence'i tekrar dogrular.
|
||||
|
||||
Carrier register'i signed Q7 toplam NCO frekansidir. Doppler ve range-rate
|
||||
donusumu:
|
||||
|
||||
@@ -759,23 +829,28 @@ range_rate_mps = -doppler_hz * c / 1575420000
|
||||
|
||||
Ham 20 ms carrier snapshot'inda kartta gorulen birkac hertz jitter, L1'de
|
||||
metre/saniye seviyesinde range-rate gurultusune ve zayif geometride daha buyuk
|
||||
receiver velocity hatasina donusur. Bu nedenle Doppler ayni PRN icin software
|
||||
ISR state'inde:
|
||||
receiver velocity hatasina donusur. Software ISR her PRN icin ham degeri
|
||||
degistirmeden korur ve iki ayri state gunceller:
|
||||
|
||||
```text
|
||||
doppler_filtered = 0.05 * doppler_raw + 0.95 * doppler_previous
|
||||
PPS: doppler_slow = 0.05 * raw + 0.95 * previous
|
||||
ESKF: doppler_fast = alpha * raw + (1-alpha) * previous
|
||||
alpha = 0.05 stationary, 0.20 moving, 0.40 high acceleration
|
||||
```
|
||||
|
||||
ile filtrelenir ve range-rate filtrelenmis Doppler'dan tekrar hesaplanir.
|
||||
50 Hz measurement cadence'inde bu yaklasik `0.39 s` zaman sabitidir.
|
||||
Reacquisition/PRN degisiminde eski kanal state'i kullanilmaz.
|
||||
PPS yolu 50 Hz cadence'de yaklasik `0.39 s` zaman sabitine sahiptir. ESKF yolu
|
||||
fusion motion state ve gravity-compensated linear acceleration ile bant
|
||||
genisligini acar. Alpha buyudugunde EWMA noise kazanci kadar WLS range-rate
|
||||
sigma da buyutulur; bu, FDE ve ESKF covariance'in filtre bant genisliginden
|
||||
habersiz kalmasini engeller. Reacquisition/PRN degisiminde eski kanal state'i
|
||||
kullanilmaz.
|
||||
|
||||
IIR donanimda da uygulanabilirdi; bunun icin kanal basina state,
|
||||
coefficient-multiply, rounding/saturation ve reacquisition reset kurali
|
||||
gerekir. Mevcut sistemde gerekli degildir: RTL'nin gorevi ham, atomik ve
|
||||
20 ms sabit olcumu sunmaktir; C/N0/WLS zaten software'dedir, katsayi hareket
|
||||
profiline gore bitstream almadan degistirilebilir ve ham Doppler debug icin
|
||||
korunur. Board testinde CPU/interrupt yolu measurement kaybetmedigi icin
|
||||
20 ms sabit olcumu sunmaktir; C/N0/WLS zaten software'dedir, ham ve filtreli
|
||||
yollar bitstream almadan ayrilabilir. Board testinde CPU/interrupt yolu
|
||||
measurement kaybetmedigi icin
|
||||
RTL filtresinin karsilayacagi bir bant genisligi veya latency problemi de
|
||||
gorulmemistir. Ileride tamamen RTL icinde bir tuketici eklenirse ham register
|
||||
yaninda ayri bir filtered register mantikli olabilir.
|
||||
@@ -792,20 +867,71 @@ sigma_rate = clamp(30 * 10^(-C/N0_dbhz / 20), 0.05, 5.0)
|
||||
weight = 1 / sigma_rate^2
|
||||
```
|
||||
|
||||
Her 200 ms navigation epoch'unda iki velocity tuketicisi vardir:
|
||||
|
||||
```text
|
||||
adaptive range-rate -> robust velocity WLS -> fusion adapter -> ESKF
|
||||
slow range-rate -> robust velocity WLS -> PPS bias-drift predictor
|
||||
```
|
||||
|
||||
Tam ham 20 ms snapshot yolu kart A/B'sinde fazla gurultulu bulundugu icin
|
||||
ESKF'ye yayinlanmaz. Adaptif yol sakin durumda PPS kadar dusuk gurultulu,
|
||||
harekette ise sabit `alpha=0.05` yolundan daha dusuk gecikmelidir.
|
||||
PPS'in ana olcumu position WLS `clock_bias_m` sonucudur. Filtreli velocity
|
||||
yalniz bu referans-datum bias'inin 200 ms epoch'lar arasindaki turevini
|
||||
ongormek ve PPS kalite profilini siniflandirmak icin kullanilir. Doppler
|
||||
WLS'nin dorduncu `receiver_clock_drift_mps` state'i PPS'e dogrudan verilmez.
|
||||
|
||||
Position WLS ile Doppler WLS iki ayri fiziksel observation kullanir:
|
||||
code-phase pseudorange konum/ofseti, carrier Doppler ise hiz/frekans hatasini
|
||||
olcer. Klasik mutlak-pseudorange modelinde ayni receiver zaman hatasi
|
||||
`delta_t_r(t)` icin:
|
||||
|
||||
```text
|
||||
code WLS: [x, y, z, b_r], b_r = c * delta_t_r [m]
|
||||
Doppler WLS: [vx, vy, vz, bdot_r], bdot_r = c * d(delta_t_r)/dt [m/s]
|
||||
```
|
||||
|
||||
Doppler WLS'nin dorduncu bilinmeyeni receiver clock/frequency drift'idir.
|
||||
Receiver oscillator `0.1 ppm` hizliysa `bdot_r` yaklasik `29.98 m/s` olur.
|
||||
Bu etki butun uydu Doppler'larina ortak girer; cozulmezse bir kismi
|
||||
`vx/vy/vz` receiver hareketi sanilir.
|
||||
|
||||
Bu projedeki position WLS'e verilen capture zamanlarindan ortak receiver
|
||||
zamani cikarilir ve `STARTOFFSET` eklenir. Dolayisiyla
|
||||
`solution.clock_bias_m`, saf `c*delta_t_r` yerine yaklasik
|
||||
`c*STARTOFFSET - reference_range` olan referans-uyduya bagli goreli bir
|
||||
datumdur. Doppler `solution.clock_drift_mps` ile bunun arasinda turev kisiti
|
||||
kurulmaz.
|
||||
|
||||
Fit'ler matematiksel olarak ayridir ve `solution_valid`/`velocity_valid`
|
||||
bagimsizdir. Bununla birlikte velocity WLS LOS birim vektorlerini hesaplamak
|
||||
icin position WLS'nin guncel receiver ECEF konumunu kullanir; uydu ECEF
|
||||
konumlari, hizlari ve satellite clock drift'leri broadcast ephemeristen gelir.
|
||||
Position kabul edilip velocity reddedilebilir; velocity NIS reject ayni
|
||||
epoch'taki position update'ini geri almaz.
|
||||
|
||||
En az dort uydu gerekir. Dort uyduda velocity residual serbestlik derecesi
|
||||
yoktur; bes uydu ve dusuk PDOP'ta bile zayif geometri velocity gurultusunu
|
||||
buyutebilir. WLS, `inverse(H'WH)` matrisinden ECEF velocity covariance uretir.
|
||||
`N>4` ise weighted post-fit reduced chi-square yalniz covariance'i buyutebilir;
|
||||
tek bir sessiz epoch formal belirsizligi kucultemez. Sonuc `velocity_valid`,
|
||||
ECEF velocity/covariance, `speed_mps` ve `velocity_residual_rms_mps`
|
||||
alanlarinda yayinlanir.
|
||||
buyutebilir. Alti veya daha fazla kullanilabilir uyduda full-fit maksimum
|
||||
normalized residual `3.5 sigma`'yi asarsa butun tek-kanal leave-one-out
|
||||
adaylari cozulur; en dusuk weighted post-fit residual enerjili aday secilir.
|
||||
Boylece ESKF'nin aggregate 3D velocity NIS kapisina gelmeden once bozuk kanal
|
||||
ayrilabilir. Dort/bes uyduda redundancy yetersiz oldugu icin velocity FDE
|
||||
uygulanmaz.
|
||||
|
||||
WLS, `inverse(H'WH)` matrisinden ECEF velocity covariance uretir. `N>4` ise
|
||||
weighted post-fit reduced chi-square yalniz covariance'i buyutebilir; tek bir
|
||||
sessiz epoch formal belirsizligi kucultemez. Sonuc `velocity_valid`, ECEF
|
||||
velocity/covariance, `speed_mps`, `velocity_residual_rms_mps`,
|
||||
`velocity_fde_applied` ve `velocity_excluded_row_mask` alanlarinda yayinlanir.
|
||||
|
||||
Fusion adapter covariance'i `R_ecef_to_enu * P * R^T` ile ENU'ya dondurur.
|
||||
2026-07-25 sabit kart verisinde zayif geometriyle gorulen
|
||||
`p95=6.020 m/s` speed hatasi nedeniyle default alt sinirlar yatay eksenlerde
|
||||
`2.0 m/s`, dikey eksende `3.0 m/s`'dir. Bunlar IIR ile korelasyonlu ard arda
|
||||
epoch'larin ESKF'yi asiri guvenle cekmesini onler; formal covariance daha
|
||||
buyukse aynen korunur. Velocity innovation 3-DOF `%99.9`
|
||||
`2.0 m/s`, dikey eksende `3.0 m/s`'dir. Bu konservatif tabanlar zayif geometri
|
||||
ve kartta kalan Doppler hata kuyruklarini sinirlar; formal
|
||||
covariance daha buyukse aynen korunur. Velocity innovation 3-DOF `%99.9`
|
||||
`NIS=16.266` kapisindan gecer. Kabul/red sayilari ve son velocity NIS'i
|
||||
`[FUSION]` UART status satirinda `velocity=accepted/rejected` ve `vel_nis`
|
||||
olarak yayinlanir.
|
||||
@@ -828,7 +954,9 @@ olarak yayinlanir.
|
||||
Ionosphere correction 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.
|
||||
- `POSTPONE_TOW=0.180` mevcut RTL'nin dokuzuncu takip eden navigation bitinde
|
||||
yaptigi teslimi duzeltir. `satpos()` ve PPS receive-time anchor bu degeri
|
||||
tam bir kez ve ayni firmware surumunde kullanir.
|
||||
- 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 artik tracked olmadigi icin bekleme maskesinden cikarilir; epoch ortasinda navigation-ready olan yeni kanal ise aktif maskeyi buyutmez ve sonraki epoch'tan itibaren beklenir. Kalan beklenen kanallarin tamami gelirse snapshot son interrupt'ta hemen yayinlanir. 30 ms holdoff dolarsa veya daha yeni fiziksel epoch baslarsa eksik eski epoch WLS calistirilmadan discard edilir; eski ve yeni olcumler birlestirilmez.
|
||||
- Interrupt bekleme maskesi tamamlandiktan sonra frozen kanallarin tumu ortak TOW/`ABS_SAMPLE` kumesinde degilse epoch reddedilir. Selector'in buldugu daha kucuk alt kume yalniz diagnostic bilgidir ve WLS girdisi olamaz.
|
||||
- Dort uydu 3D konum + receiver clock bias icin minimum ve yeterlidir. Bu durumda
|
||||
@@ -861,9 +989,9 @@ GPS_LOG(level, category, ...)
|
||||
|
||||
| `GPS_DEBUG_LEVEL` | Eklenen cikti |
|
||||
| ---: | --- |
|
||||
| `1` | Temel telemetri: ham GPS/WLS `[LOCATION] source=gps` satirinda DOP ile birlikte `velocity_valid`, `speed` ve `velocity_rms`; gecerliyse ESKF `[LOCATION] source=imu_corrected`; sifir tabanli donanim kanal numaralarini tasiyan `[TRK] tracked satellites=... prn_indices=[...]` (`0` = GPS PRN `G01`). Baslangicta fusion/IMU konfigurasyonu da bir kez yazilir. |
|
||||
| `1` | Temel telemetri: ham GPS/WLS `[LOCATION] source=gps` satirinda DOP ile birlikte `velocity_valid`, `speed` ve `velocity_rms`; gecerliyse ESKF `[LOCATION] source=imu_corrected`; sifir tabanli donanim kanal numaralarini tasiyan `[TRK] tracked satellites=... prn_indices=[...]` (`0` = GPS PRN `G01`). Baslangicta fusion/IMU konfigurasyonu ve periyodik `[FUSION]` NIS/kabul-red durumu da gorulur. |
|
||||
| `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. |
|
||||
| `3` | Level 1'e ek olarak acquisition/tracking/navigation durum gecisleri, warning/error mesajlari ve integrity bilgileri. |
|
||||
| `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
|
||||
@@ -1085,9 +1213,10 @@ make -C soc/software/gps_system/tests sanitize
|
||||
make -C hardware/external/gps/rtl/trck_src/tb test
|
||||
```
|
||||
|
||||
`run` ve `sanitize`, C/N0 estimator'i, Q7 Doppler/range-rate donusumu,
|
||||
coherent mailbox read'i, weighted velocity WLS/covariance ve satellite
|
||||
velocity/clock drift hesaplarini kapsar. Fusion regression'i ayrica
|
||||
`run` ve `sanitize`, C/N0 estimator'i, Q7 ham/filtreli Doppler-range-rate
|
||||
donusumu, coherent mailbox read'i, weighted velocity WLS/covariance,
|
||||
tek-kanal robust FDE ve satellite velocity/clock drift hesaplarini kapsar.
|
||||
Fusion regression'i ayrica
|
||||
ECEF->ENU velocity/covariance rotasyonunu, velocity NIS kabul/redini ve ayni
|
||||
epoch position+velocity delayed replay esdegerligini kapsar. RTL
|
||||
Questa/vlog-vsim testi prompt akumulasyonu,
|
||||
@@ -1130,10 +1259,51 @@ azaltiyor ve C/N0 weighting ornek epoch'ta sonucu iyilestiriyor. Buna ragmen
|
||||
position WLS'deki pseudorange/code-phase hata ve zayif geometriyi tek basina
|
||||
duzeltmiyor. Doppler velocity artik ESKF state update'ine baglidir; formal
|
||||
covariance, kart verisine dayali floor ve ayri NIS gate ile asiri guven
|
||||
sinirlanir. Bu yeni software'in gercek kartta `vel_nis` ve
|
||||
`velocity=accepted/rejected` sayaclariyla saha A/B sonucu henuz olculmedi;
|
||||
yukaridaki board istatistikleri ESKF entegrasyonundan onceki ham velocity
|
||||
dogrulamasidir.
|
||||
sinirlanir. Yukaridaki board istatistikleri ESKF entegrasyonundan onceki
|
||||
velocity dogrulamasidir; entegrasyon sonrasi `vel_nis` ve
|
||||
`velocity=accepted/rejected` saha sonucu bir sonraki bolumdedir.
|
||||
|
||||
### 2026-07-26 Genesys2 PPS ve Doppler/ESKF A/B
|
||||
|
||||
Kart sabitken once tam ham 20 ms snapshot -> robust WLS -> ESKF yolu olculdu.
|
||||
Son 120 PPS penceresinde ham WLS speed RMS `9.17 m/s`, p95 `16.71 m/s` ve
|
||||
raw-FDE `86/120` oldu. Yavas `alpha=0.05` PPS yolu ayni pencerede `0.92 m/s`
|
||||
RMS, `1.63 m/s` p95 ve `0/120` FDE verdi. ESKF+ZUPT fused hizi dusuk tutsa da
|
||||
bu sonuc tam ham yolun hareketli platform icin guvenli oldugunu dogrulamadi.
|
||||
|
||||
Bu nedenle ESKF icin yukarida tarif edilen ayri adaptif yol uygulandi. Ayni
|
||||
kartta 240 epoch'luk yerlesmis pencerede `alpha=0.05`, speed RMS `0.48 m/s`,
|
||||
p95 `0.83 m/s`, FDE `0/240` olculdu. Son 180 saniyede ESKF velocity kabul/red
|
||||
farki `892/0`, velocity NIS median/p95/max `0.028/0.111/0.180`, fused speed
|
||||
RMS `0.0052 m/s` oldu. IMU overflow sayaci bu pencerede `15` olarak sabit
|
||||
kaldi; olcum telemetrisi yeni kayip uretmedi.
|
||||
|
||||
PPS predictor, yeniden boot etmeden 40 saniyelik kapali/acik segmentlerle A/B
|
||||
yapildi (`GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=200`, yalniz tanisal build).
|
||||
Her segmentin ilk bes saniyesi atildiginda predictor acik segmentlerde software
|
||||
clock-bias innovation RMS `12.52 m`, kapali segmentlerde `46.48 m` oldu.
|
||||
Hardware phase p95 acikken `51.02 ns`, kapaliyken `58.21 ns` idi. Tek acik
|
||||
segmentte gorulen bir `-216.48 ns` one-epoch phase transient'i lock kaybi
|
||||
olusturmadi; bu nedenle physical-pin jitter sonucu olarak yorumlanmaz.
|
||||
Production default predictor acik, A/B toggling kapali (`0`) kalir.
|
||||
|
||||
Son olarak `adaptive-pps-final-20260726` production image'i
|
||||
`PREDICTOR_ENABLE=1`, `PREDICTOR_AB_UPDATES=0` ile RAM'den karta yuklendi.
|
||||
Acquisition/coarse baslangici dislandiktan sonraki 120 lock status orneginde:
|
||||
|
||||
- predictor tum orneklerde `1`, ESKF alpha sabit kartta tum orneklerde `0.05`,
|
||||
- adaptive velocity-WLS speed RMS/p95 `0.52/0.92 m/s`, FDE `0/120`,
|
||||
- ESKF velocity kabul/red farki `448/0`, velocity NIS p95/max
|
||||
`0.139/0.341`, fused speed RMS `0.0061 m/s`,
|
||||
- PPS software clock innovation RMS `12.36 m`,
|
||||
- internal hardware phase RMS ve p95-absolute `28.50/58.36 ns`, lock kaybi
|
||||
`0/120`,
|
||||
- IMU overflow sayaci `13..13`, tracking measurement missed sayaci `0..0`
|
||||
kaldi.
|
||||
|
||||
Bu `hw_phase`, APB status'taki son commit residual'idir; fiziksel PPS pin
|
||||
jitter'i degildir. Dolayisiyla `15-20 ns` pin hedefinin saglandigi bu logdan
|
||||
iddia edilmez; scope veya time-interval counter ile ayri olculmelidir.
|
||||
|
||||
### 2026-07-08 ACQ Debug Notu
|
||||
|
||||
@@ -1180,9 +1350,11 @@ Bu timer/scheduler yavasligi degildir. Firmware start ediyor; tamamlanma icin RF
|
||||
T_rx = TOW_ref + POSTPONE_TOW + STARTOFFSET/1000 - b/c
|
||||
```
|
||||
|
||||
- `POSTPONE_TOW=0.160 s`, takip eden subframe'dan parity icin beklenen sekiz
|
||||
bitin gecikmesidir ve yazilim tarafinda tam bir kez uygulanir. PPS RTL bu
|
||||
degeri tekrar eklemez.
|
||||
- `POSTPONE_TOW=0.180 s`, mevcut tracking RTL'nin parity kontrolunu dokuzuncu
|
||||
takip eden bitte tamamlamasinin gecikmesidir ve yazilim tarafinda tam bir kez
|
||||
uygulanir. PPS RTL bu degeri tekrar eklemez. Tracking RTL sekizinci bitte
|
||||
validate edecek sekilde degistirilirse bu sabit ayni release'de 160 ms'ye
|
||||
geri alinmalidir.
|
||||
- Bu GPS zamani, ayni kalici referans kanalinin 125 MHz capture counter'i ile atomik
|
||||
APB mailbox uzerinden eslenir. Ilk cozum coarse-align, sonraki cozumler Q2.46
|
||||
dusuk bant genislikli PI rate discipline yapar. WLS/APB calisma suresi faz
|
||||
@@ -1208,8 +1380,10 @@ T_rx = TOW_ref + POSTPONE_TOW + STARTOFFSET/1000 - b/c
|
||||
diye `1.024 us` tutulur. Watchdog, quality holdover ve `20 ppm` slew clamp
|
||||
buyuk arizalari ayri olarak sinirlar.
|
||||
- Yazilim filtresi PI'nin yerine gecmez. Yalnizca 5 Hz WLS clock-bias
|
||||
olcumunde Doppler clock-drift prediction, bounded innovation ve aykiri
|
||||
olcum reddi yapar. Ilk yerlesmede commit'ler coarse ve PPS invalid tutulur.
|
||||
olcumunde filtreli-Doppler velocity'den turetilen referans-datum drift
|
||||
prediction, bounded innovation ve aykiri olcum reddi yapar. Position WLS
|
||||
`clock_bias_m` ana PPS olcumu olarak kalir; Doppler yalnız prediction'dir.
|
||||
Ilk yerlesmede commit'ler coarse ve PPS invalid tutulur.
|
||||
Periyodik background watchdog iki saniye yeni referans gelmezse cikis
|
||||
validligini holdover ile kapatir; fakat ayni referans geri gelirse Doppler
|
||||
prediction ve donanim PI state'i 30 saniyeye kadar korunur. Referans SVID
|
||||
|
||||
@@ -29,6 +29,22 @@
|
||||
#define IMU_SAMPLES_BEFORE_GPS_SERVICE 1u
|
||||
#define IMU_SAMPLES_AFTER_GPS_SERVICE 3u
|
||||
|
||||
static void update_eskf_doppler_bandwidth(
|
||||
const navigation_fusion_output_t* output)
|
||||
{
|
||||
if((output == NULL) || !output->local_valid)
|
||||
{
|
||||
return;
|
||||
}
|
||||
double alpha = 0.05;
|
||||
if(output->motion_state == FUSION_MOTION_MOVING)
|
||||
{
|
||||
alpha = (output->linear_acceleration_norm_mps2 >= 2.5) ?
|
||||
0.40 : 0.20;
|
||||
}
|
||||
gps_receiver_set_eskf_doppler_alpha(alpha);
|
||||
}
|
||||
|
||||
static void initialize_sensor_fusion(gps_system_state_t* system)
|
||||
{
|
||||
system->fusion_enabled = (GPS_ESKF_ENABLE != 0);
|
||||
@@ -69,9 +85,13 @@ static unsigned int feed_pending_imu_to_eskf(
|
||||
imu_sensor_take_measurement(&system->imu_sample))
|
||||
{
|
||||
bool accepted = navigation_fusion_push_imu(&system->imu_sample);
|
||||
if(accepted &&
|
||||
navigation_fusion_get_output(&system->fused_solution) &&
|
||||
system->fused_solution.global_valid &&
|
||||
bool output_available = accepted &&
|
||||
navigation_fusion_get_output(&system->fused_solution);
|
||||
if(output_available)
|
||||
{
|
||||
update_eskf_doppler_bandwidth(&system->fused_solution);
|
||||
}
|
||||
if(output_available && system->fused_solution.global_valid &&
|
||||
(system->fused_solution.timestamp_ticks !=
|
||||
system->last_fused_location_tick))
|
||||
{
|
||||
|
||||
@@ -19,6 +19,7 @@ typedef struct {
|
||||
double height_m;
|
||||
double position_std_m[3];
|
||||
double speed_mps;
|
||||
double linear_acceleration_norm_mps2;
|
||||
double last_position_nis;
|
||||
double last_velocity_nis;
|
||||
fusion_motion_state_t motion_state;
|
||||
|
||||
@@ -32,8 +32,12 @@
|
||||
double elevation_deg[GPS_TRCK_IP_COUNT];
|
||||
double measurement_sigma_m[GPS_TRCK_IP_COUNT];
|
||||
double cn0_dbhz[GPS_TRCK_IP_COUNT];
|
||||
/* Raw Doppler is published with the ESKF velocity solution. */
|
||||
double doppler_hz[GPS_TRCK_IP_COUNT];
|
||||
double pseudorange_rate_mps[GPS_TRCK_IP_COUNT];
|
||||
/* Separate slow path used only by the PPS bias-drift predictor. */
|
||||
double filtered_doppler_hz[GPS_TRCK_IP_COUNT];
|
||||
double filtered_pseudorange_rate_mps[GPS_TRCK_IP_COUNT];
|
||||
double full_postfit_residual_m[GPS_TRCK_IP_COUNT];
|
||||
double full_elevation_deg[GPS_TRCK_IP_COUNT];
|
||||
double full_measurement_sigma_m[GPS_TRCK_IP_COUNT];
|
||||
@@ -53,6 +57,7 @@
|
||||
unsigned int degrees_of_freedom;
|
||||
uint32_t used_row_mask;
|
||||
uint32_t excluded_row_mask;
|
||||
uint32_t velocity_excluded_row_mask;
|
||||
uint64_t candidate_cycles;
|
||||
uint64_t full_solution_cycles;
|
||||
uint64_t fde_cycles;
|
||||
@@ -65,6 +70,7 @@
|
||||
bool fde_applied;
|
||||
bool solution_valid;
|
||||
bool velocity_valid;
|
||||
bool velocity_fde_applied;
|
||||
} gps_nav_solution_t;
|
||||
|
||||
void satpos(gps_ephemeris_t* eph, double* pos, double* vel, double* clk, double* clockCorr);
|
||||
|
||||
@@ -55,6 +55,8 @@
|
||||
void gps_background_task(void);
|
||||
bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot);
|
||||
void gps_get_runtime_diagnostics(gps_runtime_diagnostics_t* diagnostics);
|
||||
void gps_tracking_set_eskf_doppler_alpha(double alpha);
|
||||
double gps_tracking_get_eskf_doppler_alpha(void);
|
||||
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);
|
||||
|
||||
|
||||
|
||||
@@ -55,6 +55,7 @@
|
||||
#endif
|
||||
#define GPS_NAV_BITS_PER_WEEK 30240000u
|
||||
#define GPS_NAV_SOLVE_PERIOD_MS (GPS_NAV_SOLVE_TIMER_TICKS * GPS_NAV_SOLVE_TIMER_TICK_MS)
|
||||
#define GPS_NAV_BITS_PER_SUBFRAME 300u
|
||||
/* Keep integrity statistics active, but do not let leave-one-out FDE
|
||||
* replace the full-WLS position until the real measurement-noise model
|
||||
* is calibrated. The live stationary test produced a 22.7% exclusion
|
||||
@@ -83,6 +84,8 @@
|
||||
#error "GPS_NAV_SOLVE_TIMER_TICKS must be non-zero"
|
||||
#elif ((GPS_NAV_BITS_PER_WEEK % GPS_NAV_SOLVE_TIMER_TICKS) != 0u)
|
||||
#error "GPS_NAV_SOLVE_TIMER_TICKS must divide 30240000 GPS navigation bits per week"
|
||||
#elif ((GPS_NAV_BITS_PER_SUBFRAME % GPS_NAV_SOLVE_TIMER_TICKS) != 0u)
|
||||
#error "GPS_NAV_SOLVE_TIMER_TICKS must divide the 300-bit LNAV subframe"
|
||||
#endif
|
||||
_Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u),
|
||||
"GPS_NAV_ENABLE_FDE must be zero or one");
|
||||
@@ -174,7 +177,10 @@
|
||||
|
||||
// gps defines from plutogpssim.
|
||||
|
||||
#define POSTPONE_TOW (double)(0.160)
|
||||
/* Current tracking RTL validates the subframe on the ninth following
|
||||
* navigation bit: 9 * 20 ms = 180 ms. satpos() and the PPS receive-time
|
||||
* anchor must use this same delivery-epoch correction exactly once. */
|
||||
#define POSTPONE_TOW (double)(0.180)
|
||||
#define MAX_SAT (12) // 32 (TODO. OBSOLATED)
|
||||
|
||||
#define SECONDS_IN_WEEK 604800.0
|
||||
|
||||
@@ -2,6 +2,9 @@
|
||||
#define GPS_PPS_QUALITY_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define GPS_PPS_BASELINE_UPDATE_PERIOD_SEC 0.200
|
||||
|
||||
typedef enum {
|
||||
GPS_PPS_PROFILE_HOLDOVER = 0,
|
||||
@@ -29,6 +32,8 @@ typedef struct {
|
||||
gps_pps_profile_t profile;
|
||||
gps_pps_profile_t candidate;
|
||||
unsigned int candidate_updates;
|
||||
unsigned int upgrade_updates;
|
||||
unsigned int degrade_updates;
|
||||
} gps_pps_quality_selector_t;
|
||||
|
||||
typedef struct {
|
||||
@@ -43,13 +48,17 @@ typedef struct {
|
||||
} gps_pps_loop_profile_t;
|
||||
|
||||
void gps_pps_quality_selector_init(gps_pps_quality_selector_t* selector);
|
||||
void gps_pps_quality_selector_init_period(
|
||||
gps_pps_quality_selector_t* selector,
|
||||
double update_period_sec);
|
||||
|
||||
/*
|
||||
* Select a bounded loop profile from timing geometry, code/carrier quality
|
||||
* and integrity. Upgrades require sustained evidence and degradations require
|
||||
* three consecutive observations. A single rejected WLS epoch is skipped
|
||||
* without resetting the loop; sustained loss enters holdover, while the
|
||||
* independent reference watchdog remains the final stale-data guard.
|
||||
* and integrity. Upgrades and degradations require sustained evidence; their
|
||||
* update counts are scaled from the proven 200 ms timing when period-aware
|
||||
* initialization is used. A single rejected WLS epoch is skipped without
|
||||
* resetting the loop; sustained loss enters holdover, while the independent
|
||||
* reference watchdog remains the final stale-data guard.
|
||||
*/
|
||||
gps_pps_profile_t gps_pps_quality_selector_update(
|
||||
gps_pps_quality_selector_t* selector,
|
||||
@@ -57,6 +66,22 @@ gps_pps_profile_t gps_pps_quality_selector_update(
|
||||
bool* profile_changed_out);
|
||||
|
||||
gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile);
|
||||
gps_pps_loop_profile_t gps_pps_loop_profile_for_period(
|
||||
gps_pps_profile_t profile,
|
||||
double update_period_sec);
|
||||
|
||||
/*
|
||||
* Map the proven 200 ms discrete poles to another update period. The pole
|
||||
* exponentiation preserves the current loop's continuous-time settling and
|
||||
* damping while returning exactly the legacy coefficients at 200 ms.
|
||||
*/
|
||||
bool gps_pps_loop_coefficients(
|
||||
gps_pps_loop_profile_t parameters,
|
||||
double update_period_sec,
|
||||
double clock_frequency_hz,
|
||||
int64_t* kp_q46_out,
|
||||
int64_t* ki_q46_out,
|
||||
uint32_t* max_slew_q32_out);
|
||||
const char* gps_pps_profile_name(gps_pps_profile_t profile);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -58,7 +58,9 @@ bool gps_pps_reference_bias_drift(
|
||||
* linear oscillator drift. The Doppler WLS clock-drift state predicts the
|
||||
* bias at the new capture epoch; only the residual bias noise is low-passed.
|
||||
* If Doppler is temporarily unavailable, a bounded, slower finite-difference
|
||||
* drift estimate keeps the filter usable in the robust profile.
|
||||
* drift estimate keeps the filter usable in the robust profile. All three
|
||||
* baseline per-update gains are converted from the actual elapsed time, so
|
||||
* changing the NAV solve period does not change their real-time poles.
|
||||
*
|
||||
* COARSE is returned during initial settling, a reference-datum change, or
|
||||
* after a gap too long for bounded Doppler prediction. The independent
|
||||
|
||||
@@ -38,4 +38,9 @@ bool gps_receiver_solve_ready_epoch(
|
||||
uint64_t gps_receiver_status_ticks(void);
|
||||
uint64_t gps_receiver_status_ticks_per_second(void);
|
||||
|
||||
/* Select the low-latency Doppler EWMA used only by velocity WLS/ESKF.
|
||||
* PPS keeps its independent fixed low-noise path. */
|
||||
void gps_receiver_set_eskf_doppler_alpha(double alpha);
|
||||
double gps_receiver_get_eskf_doppler_alpha(void);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -6,6 +6,8 @@
|
||||
|
||||
#include "gps/gps_ip_defines.h"
|
||||
|
||||
#define GPS_TRACK_PPS_DOPPLER_ALPHA 0.05
|
||||
|
||||
typedef struct {
|
||||
uint32_t prn;
|
||||
uint32_t sequence;
|
||||
@@ -17,8 +19,15 @@ typedef struct {
|
||||
int32_t cn0_sum_q;
|
||||
uint64_t cn0_wbp;
|
||||
unsigned int cn0_sample_count;
|
||||
/* Raw coherent 20 ms carrier snapshot retained for diagnostics. */
|
||||
double doppler_hz;
|
||||
double pseudorange_rate_mps;
|
||||
/* Motion-adaptive low-latency path published to velocity WLS/ESKF. */
|
||||
double eskf_doppler_hz;
|
||||
double eskf_pseudorange_rate_mps;
|
||||
/* Slow software IIR path retained only for PPS bias-drift prediction. */
|
||||
double filtered_doppler_hz;
|
||||
double filtered_pseudorange_rate_mps;
|
||||
double cn0_dbhz;
|
||||
bool nav_bit;
|
||||
bool locked;
|
||||
@@ -59,4 +68,23 @@ bool gps_tracking_velocity_wls(
|
||||
double* receiver_clock_drift_mps_out,
|
||||
double* residual_rms_mps_out);
|
||||
|
||||
/*
|
||||
* Raw-Doppler velocity solution with a one-channel leave-one-out FDE trial.
|
||||
* FDE runs only for six or more usable channels and only when the full-fit
|
||||
* maximum normalized residual exceeds 3.5 sigma. The returned bit mask uses
|
||||
* indices from the input measurement array, not compacted WLS row indices.
|
||||
*/
|
||||
bool gps_tracking_velocity_wls_robust(
|
||||
const double receiver_ecef_m[3],
|
||||
const double satellite_positions_m[][3],
|
||||
const double satellite_velocities_mps[][3],
|
||||
const double satellite_clock_drifts_sps[],
|
||||
const gps_tracking_measurement_t measurements[],
|
||||
unsigned int count,
|
||||
double receiver_velocity_ecef_mps_out[3],
|
||||
double receiver_velocity_covariance_ecef_mps2_out[3][3],
|
||||
double* receiver_clock_drift_mps_out,
|
||||
double* residual_rms_mps_out,
|
||||
uint32_t* excluded_measurement_mask_out);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -27,12 +27,13 @@ void gps_system_uart_publish_location(
|
||||
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION,
|
||||
"source=%s lat=%.20lf lon=%.20lf height=%.3lf "
|
||||
"gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf "
|
||||
"tracked=%u used=%u velocity_valid=%u speed=%.3lf "
|
||||
"tracked=%u used=%u velocity_valid=%u velocity_fde=%u speed=%.3lf "
|
||||
"velocity_rms=%.3lf\r\n",
|
||||
source, latitude_deg, longitude_deg, height_m,
|
||||
gps->gdop, gps->pdop, gps->hdop, gps->vdop, gps->tdop,
|
||||
gps->tracked_satellite_count, gps->used_satellite_count,
|
||||
gps->velocity_valid ? 1u : 0u,
|
||||
gps->velocity_fde_applied ? 1u : 0u,
|
||||
gps->speed_mps, gps->velocity_residual_rms_mps);
|
||||
}
|
||||
|
||||
@@ -114,7 +115,7 @@ void gps_system_uart_publish_periodic_fusion_status(
|
||||
|
||||
if(output->global_valid)
|
||||
{
|
||||
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
|
||||
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
|
||||
"state=%s lat=%.10lf lon=%.10lf height=%.3lf "
|
||||
"speed=%.3lf pos_nis=%.3lf vel_nis=%.3lf "
|
||||
"gps=%lu/%lu velocity=%lu/%lu recoveries=%lu "
|
||||
@@ -141,7 +142,7 @@ void gps_system_uart_publish_periodic_fusion_status(
|
||||
return;
|
||||
}
|
||||
|
||||
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
|
||||
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
|
||||
"state=%s calibration_samples=%u gps=%lu/%lu recoveries=%lu "
|
||||
"imu=%lu fused_imu=%lu overflow=%lu missed=%lu\r\n",
|
||||
navigation_fusion_state_name(output->engine_state),
|
||||
|
||||
@@ -141,6 +141,8 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output)
|
||||
sqrt(fmax(0.0, runtime->filter.covariance[axis][axis]));
|
||||
}
|
||||
output->speed_mps = eskf_speed_mps(&runtime->filter);
|
||||
output->linear_acceleration_norm_mps2 =
|
||||
runtime->motion.last_linear_accel_norm_mps2;
|
||||
output->last_position_nis = runtime->filter.last_position_nis;
|
||||
output->last_velocity_nis = runtime->filter.last_velocity_nis;
|
||||
output->motion_state = runtime->motion.state;
|
||||
|
||||
@@ -9,6 +9,7 @@
|
||||
#include <bsp/irq.h>
|
||||
#include <bsp/mmio.h>
|
||||
#include <bsp/plic.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
typedef enum {
|
||||
@@ -91,9 +92,13 @@ static gps_external_irq_handler_t gps_external_irq_handler = NULL;
|
||||
(((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)
|
||||
#define GPS_TRACK_DOPPLER_SMOOTH_ALPHA 0.05
|
||||
#define GPS_TRACK_ESKF_DOPPLER_ALPHA_MIN 0.05
|
||||
#define GPS_TRACK_ESKF_DOPPLER_ALPHA_MAX 0.40
|
||||
#define GPS_NAV_TOW_SNAP_TOLERANCE_SEC 0.005
|
||||
|
||||
static volatile double gps_tracking_eskf_doppler_alpha =
|
||||
GPS_TRACK_ESKF_DOPPLER_ALPHA_MIN;
|
||||
|
||||
_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");
|
||||
@@ -112,6 +117,22 @@ static unsigned int gps_mask_count(uint32_t mask)
|
||||
return count;
|
||||
}
|
||||
|
||||
void gps_tracking_set_eskf_doppler_alpha(double alpha)
|
||||
{
|
||||
if(!isfinite(alpha))
|
||||
{
|
||||
return;
|
||||
}
|
||||
gps_tracking_eskf_doppler_alpha = fmax(
|
||||
GPS_TRACK_ESKF_DOPPLER_ALPHA_MIN,
|
||||
fmin(GPS_TRACK_ESKF_DOPPLER_ALPHA_MAX, alpha));
|
||||
}
|
||||
|
||||
double gps_tracking_get_eskf_doppler_alpha(void)
|
||||
{
|
||||
return gps_tracking_eskf_doppler_alpha;
|
||||
}
|
||||
|
||||
static double gps_correct_abs_sample(uint32_t abs_sample, uint32_t rem_code_phase)
|
||||
{
|
||||
return (double)abs_sample - ((double)rem_code_phase / CODE_FREQ_BASIS);
|
||||
@@ -156,13 +177,20 @@ static void gps_update_tracking_measurement_isr(
|
||||
* A 0.05 pole at the 50 Hz navigation-bit cadence has an approximately
|
||||
* 0.4 s time constant while retaining real receiver dynamics.
|
||||
*/
|
||||
measurement.doppler_hz =
|
||||
GPS_TRACK_DOPPLER_SMOOTH_ALPHA * measurement.doppler_hz +
|
||||
(1.0 - GPS_TRACK_DOPPLER_SMOOTH_ALPHA) *
|
||||
previous->doppler_hz;
|
||||
measurement.pseudorange_rate_mps =
|
||||
measurement.filtered_doppler_hz =
|
||||
GPS_TRACK_PPS_DOPPLER_ALPHA * measurement.doppler_hz +
|
||||
(1.0 - GPS_TRACK_PPS_DOPPLER_ALPHA) *
|
||||
previous->filtered_doppler_hz;
|
||||
measurement.filtered_pseudorange_rate_mps =
|
||||
gps_tracking_doppler_to_range_rate_mps(
|
||||
measurement.doppler_hz);
|
||||
measurement.filtered_doppler_hz);
|
||||
double eskf_alpha = gps_tracking_eskf_doppler_alpha;
|
||||
measurement.eskf_doppler_hz =
|
||||
eskf_alpha * measurement.doppler_hz +
|
||||
(1.0 - eskf_alpha) * previous->eskf_doppler_hz;
|
||||
measurement.eskf_pseudorange_rate_mps =
|
||||
gps_tracking_doppler_to_range_rate_mps(
|
||||
measurement.eskf_doppler_hz);
|
||||
}
|
||||
measurement.local_timestamp_ticks = local_timestamp_ticks;
|
||||
gps_latest_tracking_measurement[tracking_index] = measurement;
|
||||
|
||||
@@ -69,11 +69,25 @@ static bool gps_pps_reference_svid_valid;
|
||||
static unsigned int gps_pps_reference_svid;
|
||||
static bool gps_pps_filter_reference_svid_valid;
|
||||
static unsigned int gps_pps_filter_reference_svid;
|
||||
#if GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES > 0
|
||||
static uint64_t gps_pps_predictor_ab_update_counter;
|
||||
#endif
|
||||
|
||||
#if !defined(GPS_PPS_STATUS_LOG_INTERVAL) || (GPS_PPS_STATUS_LOG_INTERVAL < 1)
|
||||
#error "GPS_PPS_STATUS_LOG_INTERVAL must be at least one"
|
||||
#endif
|
||||
|
||||
#if !defined(GPS_PPS_DOPPLER_PREDICTOR_ENABLE) || \
|
||||
((GPS_PPS_DOPPLER_PREDICTOR_ENABLE != 0) && \
|
||||
(GPS_PPS_DOPPLER_PREDICTOR_ENABLE != 1))
|
||||
#error "GPS_PPS_DOPPLER_PREDICTOR_ENABLE must be zero or one"
|
||||
#endif
|
||||
|
||||
#if !defined(GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES) || \
|
||||
(GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES < 0)
|
||||
#error "GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES must be zero or positive"
|
||||
#endif
|
||||
|
||||
volatile uint64_t gps_systick_ticks = 0;
|
||||
|
||||
static void gps_init_acq_tables(void)
|
||||
@@ -853,6 +867,12 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
||||
bool ok = false;
|
||||
bool pps_clock_bias_drift_valid = false;
|
||||
double pps_clock_bias_drift_mps = 0.0;
|
||||
bool pps_velocity_valid = false;
|
||||
double eskf_doppler_alpha =
|
||||
gps_tracking_get_eskf_doppler_alpha();
|
||||
double pps_velocity_speed_mps = NAN;
|
||||
double pps_velocity_residual_rms_mps = 0.0;
|
||||
uint32_t pps_velocity_excluded_row_mask = 0u;
|
||||
gps_nav_solution_t solution;
|
||||
memset(&solution, 0, sizeof(solution));
|
||||
memset(solution_out, 0, sizeof(*solution_out));
|
||||
@@ -954,9 +974,20 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
||||
solution.doppler_hz[row] = measurements[row].doppler_hz;
|
||||
solution.pseudorange_rate_mps[row] =
|
||||
measurements[row].pseudorange_rate_mps;
|
||||
solution.filtered_doppler_hz[row] =
|
||||
measurements[row].filtered_doppler_hz;
|
||||
solution.filtered_pseudorange_rate_mps[row] =
|
||||
measurements[row].filtered_pseudorange_rate_mps;
|
||||
}
|
||||
if(ok && solution.solution_valid)
|
||||
{
|
||||
double reference_noise_ratio =
|
||||
eskf_doppler_alpha *
|
||||
(2.0 - GPS_TRACK_PPS_DOPPLER_ALPHA) /
|
||||
(GPS_TRACK_PPS_DOPPLER_ALPHA *
|
||||
(2.0 - eskf_doppler_alpha));
|
||||
double eskf_cn0_penalty_db =
|
||||
10.0 * log10(fmax(1.0, reference_noise_ratio));
|
||||
double satellite_positions_array[GPS_TRCK_IP_COUNT][3];
|
||||
double satellite_velocities_array[GPS_TRCK_IP_COUNT][3];
|
||||
gps_tracking_measurement_t
|
||||
@@ -971,12 +1002,18 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
||||
velocities->matrix[row][component];
|
||||
}
|
||||
velocity_measurements[row] = measurements[row];
|
||||
velocity_measurements[row].doppler_hz =
|
||||
measurements[row].eskf_doppler_hz;
|
||||
velocity_measurements[row].pseudorange_rate_mps =
|
||||
measurements[row].eskf_pseudorange_rate_mps;
|
||||
velocity_measurements[row].cn0_dbhz -=
|
||||
eskf_cn0_penalty_db;
|
||||
if((solution.used_row_mask & (1u << row)) == 0u)
|
||||
{
|
||||
velocity_measurements[row].valid = false;
|
||||
}
|
||||
}
|
||||
solution.velocity_valid = gps_tracking_velocity_wls(
|
||||
solution.velocity_valid = gps_tracking_velocity_wls_robust(
|
||||
solution.ecef_m,
|
||||
satellite_positions_array,
|
||||
satellite_velocities_array,
|
||||
@@ -986,7 +1023,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
||||
solution.velocity_ecef_mps,
|
||||
solution.velocity_covariance_ecef_mps2,
|
||||
&solution.clock_drift_mps,
|
||||
&solution.velocity_residual_rms_mps);
|
||||
&solution.velocity_residual_rms_mps,
|
||||
&solution.velocity_excluded_row_mask);
|
||||
solution.velocity_fde_applied =
|
||||
solution.velocity_excluded_row_mask != 0u;
|
||||
if(solution.velocity_valid)
|
||||
{
|
||||
solution.speed_mps =
|
||||
@@ -996,15 +1036,64 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
||||
solution.velocity_ecef_mps[1] +
|
||||
solution.velocity_ecef_mps[2] *
|
||||
solution.velocity_ecef_mps[2]);
|
||||
}
|
||||
|
||||
/*
|
||||
* PPS keeps the existing slow Doppler path as a low-noise derivative
|
||||
* predictor for the position-WLS reference datum. This result is not
|
||||
* published to fusion; fusion receives the adaptive low-latency
|
||||
* solution above.
|
||||
*/
|
||||
for(unsigned int row = 0u; row < satellite_count; row++)
|
||||
{
|
||||
velocity_measurements[row].cn0_dbhz =
|
||||
measurements[row].cn0_dbhz;
|
||||
velocity_measurements[row].doppler_hz =
|
||||
measurements[row].filtered_doppler_hz;
|
||||
velocity_measurements[row].pseudorange_rate_mps =
|
||||
measurements[row].filtered_pseudorange_rate_mps;
|
||||
}
|
||||
double pps_velocity_ecef_mps[3];
|
||||
double pps_velocity_covariance_ecef_mps2[3][3];
|
||||
double pps_receiver_clock_drift_mps = 0.0;
|
||||
pps_velocity_valid = gps_tracking_velocity_wls_robust(
|
||||
solution.ecef_m,
|
||||
satellite_positions_array,
|
||||
satellite_velocities_array,
|
||||
clockDrift,
|
||||
velocity_measurements,
|
||||
satellite_count,
|
||||
pps_velocity_ecef_mps,
|
||||
pps_velocity_covariance_ecef_mps2,
|
||||
&pps_receiver_clock_drift_mps,
|
||||
&pps_velocity_residual_rms_mps,
|
||||
&pps_velocity_excluded_row_mask);
|
||||
if(pps_velocity_valid)
|
||||
{
|
||||
pps_velocity_speed_mps =
|
||||
sqrt(pps_velocity_ecef_mps[0] * pps_velocity_ecef_mps[0] +
|
||||
pps_velocity_ecef_mps[1] * pps_velocity_ecef_mps[1] +
|
||||
pps_velocity_ecef_mps[2] * pps_velocity_ecef_mps[2]);
|
||||
pps_clock_bias_drift_valid =
|
||||
gps_pps_reference_bias_drift(
|
||||
solution.ecef_m,
|
||||
solution.velocity_ecef_mps,
|
||||
pps_velocity_ecef_mps,
|
||||
satellite_positions_array[reference_index],
|
||||
satellite_velocities_array[reference_index],
|
||||
clockDrift[reference_index],
|
||||
&pps_clock_bias_drift_mps);
|
||||
}
|
||||
if(solution.velocity_fde_applied ||
|
||||
(pps_velocity_excluded_row_mask != 0u))
|
||||
{
|
||||
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
|
||||
"velocity FDE adaptive_mask=0x%02lx pps_mask=0x%02lx "
|
||||
"adaptive_rms=%.3lf pps_rms=%.3lf\r\n",
|
||||
(unsigned long)solution.velocity_excluded_row_mask,
|
||||
(unsigned long)pps_velocity_excluded_row_mask,
|
||||
solution.velocity_residual_rms_mps,
|
||||
pps_velocity_residual_rms_mps);
|
||||
}
|
||||
}
|
||||
if(ok && solution.fde_applied)
|
||||
{
|
||||
@@ -1056,12 +1145,13 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
||||
(cn0_count != 0u) ? minimum_cn0 : NAN;
|
||||
pps_quality.chi_square_ratio = solution.chi_square_ratio;
|
||||
pps_quality.velocity_residual_rms_mps =
|
||||
solution.velocity_valid ?
|
||||
solution.velocity_residual_rms_mps : 0.0;
|
||||
pps_velocity_valid ? pps_velocity_residual_rms_mps : 0.0;
|
||||
pps_quality.used_satellites = solution.used_satellite_count;
|
||||
pps_quality.residual_test_valid = solution.residual_test_valid;
|
||||
pps_quality.velocity_valid = solution.velocity_valid;
|
||||
pps_quality.fde_applied = solution.fde_applied;
|
||||
pps_quality.velocity_valid = pps_velocity_valid;
|
||||
pps_quality.fde_applied =
|
||||
solution.fde_applied ||
|
||||
(pps_velocity_excluded_row_mask != 0u);
|
||||
}
|
||||
bool pps_profile_changed = false;
|
||||
/*
|
||||
@@ -1125,11 +1215,21 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
||||
}
|
||||
double filtered_clock_bias_m = 0.0;
|
||||
double clock_innovation_m = 0.0;
|
||||
bool pps_predictor_enabled =
|
||||
(GPS_PPS_DOPPLER_PREDICTOR_ENABLE != 0) &&
|
||||
pps_clock_bias_drift_valid;
|
||||
#if GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES > 0
|
||||
pps_predictor_enabled =
|
||||
(((gps_pps_predictor_ab_update_counter /
|
||||
GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES) & 1u) != 0u) &&
|
||||
pps_clock_bias_drift_valid;
|
||||
gps_pps_predictor_ab_update_counter++;
|
||||
#endif
|
||||
gps_pps_time_filter_result_t filter_result =
|
||||
gps_pps_filter_clock_bias(
|
||||
pps_reference_counter,
|
||||
solution.clock_bias_m,
|
||||
pps_clock_bias_drift_valid,
|
||||
pps_predictor_enabled,
|
||||
pps_clock_bias_drift_mps,
|
||||
solution.tdop,
|
||||
&filtered_clock_bias_m,
|
||||
@@ -1141,7 +1241,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
||||
"drift=%.3lf tdop=%.2lf\r\n",
|
||||
gps_pps_profile_name(pps_profile),
|
||||
solution.clock_bias_m,
|
||||
solution.clock_drift_mps,
|
||||
pps_clock_bias_drift_valid ?
|
||||
pps_clock_bias_drift_mps : NAN,
|
||||
solution.tdop);
|
||||
goto pps_anchor_done;
|
||||
}
|
||||
@@ -1190,7 +1291,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
||||
"PPS profile=%s mode=%s locked=%u tdop=%.2lf gdop=%.2lf "
|
||||
"clock_sigma=%.2lf cn0=%.1lf/%.1lf innovation=%.2lfm "
|
||||
"raw_bias=%.3lfm filtered_bias=%.3lfm "
|
||||
"clock_drift=%.3lfmps bias_drift=%.3lfmps ref_svid=%u "
|
||||
"clock_drift=%.3lfmps bias_drift=%.3lfmps "
|
||||
"predictor=%u eskf_alpha=%.2lf "
|
||||
"eskf_v=%.3lf/%.3lf/0x%02lx "
|
||||
"pps_v=%.3lf/%.3lf/0x%02lx ref_svid=%u "
|
||||
"capture_corr=%.2lfns "
|
||||
"hw_phase=%.2lfns rate_cmd=%.3lfppm integ=%.3lfppm "
|
||||
"pulse=%u seq=%u status=0x%03x\r\n",
|
||||
@@ -1207,6 +1311,14 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
||||
filtered_clock_bias_m,
|
||||
solution.clock_drift_mps,
|
||||
pps_clock_bias_drift_mps,
|
||||
pps_predictor_enabled ? 1u : 0u,
|
||||
eskf_doppler_alpha,
|
||||
solution.speed_mps,
|
||||
solution.velocity_residual_rms_mps,
|
||||
(unsigned long)solution.velocity_excluded_row_mask,
|
||||
pps_velocity_speed_mps,
|
||||
pps_velocity_residual_rms_mps,
|
||||
(unsigned long)pps_velocity_excluded_row_mask,
|
||||
ephemeris[reference_index].SVID,
|
||||
pps_capture_correction_sec * 1.0e9,
|
||||
hardware_phase_ns, rate_command_ppm, integrator_ppm,
|
||||
@@ -1732,7 +1844,9 @@ void gps_startup_initialization(unsigned int nsp_tick)
|
||||
GPS_NAV_ENABLE_FDE);
|
||||
gps_init_acq_tables();
|
||||
gps_pps_init(nsp_tick);
|
||||
gps_pps_quality_selector_init(&gps_pps_quality_selector);
|
||||
gps_pps_quality_selector_init_period(
|
||||
&gps_pps_quality_selector,
|
||||
(double)nsp_tick * (double)GPS_NAV_SOLVE_TIMER_TICK_MS / 1000.0);
|
||||
gps_pps_applied_profile = GPS_PPS_PROFILE_HOLDOVER;
|
||||
gps_pps_status_log_decimator = 0u;
|
||||
gps_pps_reference_svid_valid = false;
|
||||
|
||||
@@ -65,7 +65,8 @@ enum {
|
||||
};
|
||||
|
||||
#define GPS_PPS_VERSION_ADAPTIVE_LOOP 0x00020200u
|
||||
#define GPS_PPS_REFERENCE_TIMEOUT_TICKS (2u * GPS_PPS_CLOCK_HZ)
|
||||
#define GPS_PPS_MIN_REFERENCE_TIMEOUT_TICKS (2u * GPS_PPS_CLOCK_HZ)
|
||||
#define GPS_PPS_REFERENCE_TIMEOUT_PERIODS 3u
|
||||
|
||||
static volatile gps_pps_regs_t* const gps_pps =
|
||||
(volatile gps_pps_regs_t*)BSP_GPS_PPS_BASE;
|
||||
@@ -77,6 +78,7 @@ static bool gps_pps_force_coarse;
|
||||
static bool gps_pps_reference_committed;
|
||||
static bool gps_pps_holdover;
|
||||
static uint64_t gps_pps_last_reference_counter;
|
||||
static uint64_t gps_pps_reference_timeout_ticks;
|
||||
static gps_pps_time_filter_t gps_pps_time_filter;
|
||||
|
||||
static uint64_t gps_pps_read_u64(
|
||||
@@ -94,19 +96,30 @@ static void gps_pps_write_q46(volatile uint32_t* lo, volatile uint32_t* hi, int6
|
||||
*hi = (uint32_t)(encoded >> 32);
|
||||
}
|
||||
|
||||
static void gps_pps_write_loop_profile(gps_pps_profile_t profile)
|
||||
static double gps_pps_update_period_sec(void)
|
||||
{
|
||||
const double pi = 3.14159265358979323846;
|
||||
gps_pps_loop_profile_t parameters = gps_pps_loop_profile(profile);
|
||||
const double update_period_sec = (gps_pps_nav_solve_tick != 0u) ?
|
||||
((double)gps_pps_nav_solve_tick * 0.020) : 0.200;
|
||||
const double wn = 2.0 * pi * parameters.natural_frequency_hz;
|
||||
const double q46 = 70368744177664.0;
|
||||
int64_t kp_q46 = (int64_t)llround(
|
||||
(2.0 * parameters.damping * wn / GPS_PPS_CLOCK_HZ) * q46);
|
||||
int64_t ki_q46 = (int64_t)llround((wn * wn * update_period_sec / GPS_PPS_CLOCK_HZ) * q46);
|
||||
uint32_t max_slew_q32 = (uint32_t)llround(
|
||||
parameters.max_slew_ppm * 1.0e-6 * 4294967296.0);
|
||||
return (gps_pps_nav_solve_tick != 0u) ?
|
||||
((double)gps_pps_nav_solve_tick * 0.020) :
|
||||
GPS_PPS_BASELINE_UPDATE_PERIOD_SEC;
|
||||
}
|
||||
|
||||
static bool gps_pps_write_loop_profile(gps_pps_profile_t profile)
|
||||
{
|
||||
const double update_period_sec = gps_pps_update_period_sec();
|
||||
gps_pps_loop_profile_t parameters =
|
||||
gps_pps_loop_profile_for_period(profile, update_period_sec);
|
||||
int64_t kp_q46 = 0;
|
||||
int64_t ki_q46 = 0;
|
||||
uint32_t max_slew_q32 = 0u;
|
||||
if(!gps_pps_loop_coefficients(
|
||||
parameters, update_period_sec, (double)GPS_PPS_CLOCK_HZ,
|
||||
&kp_q46, &ki_q46, &max_slew_q32))
|
||||
{
|
||||
GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_NAV,
|
||||
"PPS loop coefficient generation failed profile=%s period_ms=%.1lf\r\n",
|
||||
gps_pps_profile_name(profile), update_period_sec * 1000.0);
|
||||
return false;
|
||||
}
|
||||
|
||||
gps_pps_write_q46(&gps_pps->kp_q46_lo, &gps_pps->kp_q46_hi, kp_q46);
|
||||
gps_pps_write_q46(&gps_pps->ki_q46_lo, &gps_pps->ki_q46_hi, ki_q46);
|
||||
@@ -118,14 +131,25 @@ static void gps_pps_write_loop_profile(gps_pps_profile_t profile)
|
||||
((parameters.lock_release_ticks & 0xffu) << 16) |
|
||||
((parameters.lock_acquire_updates & 0xffu) << 24);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void gps_pps_init(unsigned int nav_solve_tick)
|
||||
{
|
||||
gps_pps_nav_solve_tick = nav_solve_tick;
|
||||
uint64_t period_ticks =
|
||||
((uint64_t)GPS_PPS_CLOCK_HZ * nav_solve_tick * 20u) / 1000u;
|
||||
gps_pps_reference_timeout_ticks =
|
||||
GPS_PPS_REFERENCE_TIMEOUT_PERIODS * period_ticks;
|
||||
if(gps_pps_reference_timeout_ticks <
|
||||
(uint64_t)GPS_PPS_MIN_REFERENCE_TIMEOUT_TICKS)
|
||||
{
|
||||
gps_pps_reference_timeout_ticks =
|
||||
GPS_PPS_MIN_REFERENCE_TIMEOUT_TICKS;
|
||||
}
|
||||
gps_pps->control = 0u;
|
||||
gps_pps_version = gps_pps->version;
|
||||
gps_pps_write_loop_profile(GPS_PPS_PROFILE_ROBUST);
|
||||
(void)gps_pps_write_loop_profile(GPS_PPS_PROFILE_ROBUST);
|
||||
gps_pps_active_loop_profile = GPS_PPS_PROFILE_ROBUST;
|
||||
/* A GDB CPU reset does not reset the tracking-clock PPS state. Continue
|
||||
* its mailbox sequence and force the first new solution to re-anchor the
|
||||
@@ -149,12 +173,14 @@ void gps_pps_init(unsigned int nav_solve_tick)
|
||||
GPS_PPS_CONTROL_CLEAR_STATUS;
|
||||
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
|
||||
"PPS init rtl=0x%08x adaptive=%u loop_config=0x%08x "
|
||||
"clock_hz=%u update_ticks=%u\r\n",
|
||||
"clock_hz=%u update_ticks=%u reference_timeout_ms=%lu\r\n",
|
||||
gps_pps_version,
|
||||
gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ? 1u : 0u,
|
||||
gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ?
|
||||
gps_pps->loop_config : 0u,
|
||||
GPS_PPS_CLOCK_HZ, gps_pps_nav_solve_tick);
|
||||
GPS_PPS_CLOCK_HZ, gps_pps_nav_solve_tick,
|
||||
(unsigned long)((gps_pps_reference_timeout_ticks * 1000u) /
|
||||
GPS_PPS_CLOCK_HZ));
|
||||
}
|
||||
|
||||
void gps_pps_service(void)
|
||||
@@ -170,7 +196,7 @@ void gps_pps_service(void)
|
||||
uint32_t now_hi = gps_pps->raw_counter_hi;
|
||||
uint64_t now = ((uint64_t)now_hi << 32) | now_lo;
|
||||
if((now - gps_pps_last_reference_counter) >
|
||||
(uint64_t)GPS_PPS_REFERENCE_TIMEOUT_TICKS)
|
||||
gps_pps_reference_timeout_ticks)
|
||||
{
|
||||
gps_pps_set_holdover(true);
|
||||
}
|
||||
@@ -229,7 +255,10 @@ bool gps_pps_set_loop_profile(gps_pps_profile_t profile)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
gps_pps_write_loop_profile(profile);
|
||||
if(!gps_pps_write_loop_profile(profile))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
gps_pps_active_loop_profile = profile;
|
||||
return true;
|
||||
}
|
||||
@@ -277,7 +306,8 @@ gps_pps_time_filter_result_t gps_pps_filter_clock_bias(
|
||||
double* innovation_m_out)
|
||||
{
|
||||
gps_pps_loop_profile_t parameters =
|
||||
gps_pps_loop_profile(gps_pps_active_loop_profile);
|
||||
gps_pps_loop_profile_for_period(
|
||||
gps_pps_active_loop_profile, gps_pps_update_period_sec());
|
||||
return gps_pps_time_filter_update(
|
||||
&gps_pps_time_filter, epoch_counter, (double)GPS_PPS_CLOCK_HZ,
|
||||
measured_clock_bias_m, clock_drift_valid,
|
||||
|
||||
@@ -7,6 +7,33 @@
|
||||
|
||||
#define GPS_PPS_PROFILE_UPGRADE_UPDATES 8u
|
||||
#define GPS_PPS_PROFILE_DEGRADE_UPDATES 3u
|
||||
#define GPS_PPS_MIN_HYSTERESIS_UPDATES 2u
|
||||
#define GPS_PPS_MIN_LOCK_UPDATES 2u
|
||||
#define GPS_PPS_MIN_SETTLE_UPDATES 3u
|
||||
#define GPS_PPS_MAX_CONFIG_UPDATES 255u
|
||||
|
||||
static unsigned int gps_pps_period_updates(
|
||||
unsigned int baseline_updates,
|
||||
double update_period_sec,
|
||||
unsigned int minimum_updates)
|
||||
{
|
||||
if(!isfinite(update_period_sec) || (update_period_sec <= 0.0))
|
||||
{
|
||||
update_period_sec = GPS_PPS_BASELINE_UPDATE_PERIOD_SEC;
|
||||
}
|
||||
double duration_sec =
|
||||
(double)baseline_updates * GPS_PPS_BASELINE_UPDATE_PERIOD_SEC;
|
||||
double required = ceil(duration_sec / update_period_sec - 1.0e-12);
|
||||
if(required < (double)minimum_updates)
|
||||
{
|
||||
return minimum_updates;
|
||||
}
|
||||
if(required > (double)GPS_PPS_MAX_CONFIG_UPDATES)
|
||||
{
|
||||
return GPS_PPS_MAX_CONFIG_UPDATES;
|
||||
}
|
||||
return (unsigned int)required;
|
||||
}
|
||||
|
||||
static bool gps_pps_metrics_finite(const gps_pps_quality_metrics_t* metrics)
|
||||
{
|
||||
@@ -67,12 +94,26 @@ static gps_pps_profile_t gps_pps_classify_quality(
|
||||
}
|
||||
|
||||
void gps_pps_quality_selector_init(gps_pps_quality_selector_t* selector)
|
||||
{
|
||||
gps_pps_quality_selector_init_period(
|
||||
selector, GPS_PPS_BASELINE_UPDATE_PERIOD_SEC);
|
||||
}
|
||||
|
||||
void gps_pps_quality_selector_init_period(
|
||||
gps_pps_quality_selector_t* selector,
|
||||
double update_period_sec)
|
||||
{
|
||||
if(selector != NULL)
|
||||
{
|
||||
memset(selector, 0, sizeof(*selector));
|
||||
selector->profile = GPS_PPS_PROFILE_HOLDOVER;
|
||||
selector->candidate = GPS_PPS_PROFILE_HOLDOVER;
|
||||
selector->upgrade_updates = gps_pps_period_updates(
|
||||
GPS_PPS_PROFILE_UPGRADE_UPDATES, update_period_sec,
|
||||
GPS_PPS_MIN_HYSTERESIS_UPDATES);
|
||||
selector->degrade_updates = gps_pps_period_updates(
|
||||
GPS_PPS_PROFILE_DEGRADE_UPDATES, update_period_sec,
|
||||
GPS_PPS_MIN_HYSTERESIS_UPDATES);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -111,8 +152,8 @@ gps_pps_profile_t gps_pps_quality_selector_update(
|
||||
|
||||
unsigned int required_updates =
|
||||
(target > selector->profile) ?
|
||||
GPS_PPS_PROFILE_UPGRADE_UPDATES :
|
||||
GPS_PPS_PROFILE_DEGRADE_UPDATES;
|
||||
selector->upgrade_updates :
|
||||
selector->degrade_updates;
|
||||
if(selector->candidate_updates >= required_updates)
|
||||
{
|
||||
selector->profile = target;
|
||||
@@ -127,10 +168,19 @@ gps_pps_profile_t gps_pps_quality_selector_update(
|
||||
|
||||
gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
|
||||
{
|
||||
return gps_pps_loop_profile_for_period(
|
||||
profile, GPS_PPS_BASELINE_UPDATE_PERIOD_SEC);
|
||||
}
|
||||
|
||||
gps_pps_loop_profile_t gps_pps_loop_profile_for_period(
|
||||
gps_pps_profile_t profile,
|
||||
double update_period_sec)
|
||||
{
|
||||
gps_pps_loop_profile_t parameters;
|
||||
switch(profile)
|
||||
{
|
||||
case GPS_PPS_PROFILE_ACQUIRE:
|
||||
return (gps_pps_loop_profile_t){
|
||||
parameters = (gps_pps_loop_profile_t){
|
||||
.natural_frequency_hz = 0.120,
|
||||
.damping = 0.9,
|
||||
.max_slew_ppm = 20.0,
|
||||
@@ -140,8 +190,9 @@ gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
|
||||
.software_bias_gain = 1.0 / 16.0,
|
||||
.software_settle_updates = 12u
|
||||
};
|
||||
break;
|
||||
case GPS_PPS_PROFILE_PRECISION:
|
||||
return (gps_pps_loop_profile_t){
|
||||
parameters = (gps_pps_loop_profile_t){
|
||||
.natural_frequency_hz = 0.035,
|
||||
.damping = 0.9,
|
||||
.max_slew_ppm = 20.0,
|
||||
@@ -151,8 +202,9 @@ gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
|
||||
.software_bias_gain = 1.0 / 32.0,
|
||||
.software_settle_updates = 12u
|
||||
};
|
||||
break;
|
||||
case GPS_PPS_PROFILE_ROBUST:
|
||||
return (gps_pps_loop_profile_t){
|
||||
parameters = (gps_pps_loop_profile_t){
|
||||
.natural_frequency_hz = 0.012,
|
||||
.damping = 1.0,
|
||||
.max_slew_ppm = 20.0,
|
||||
@@ -162,9 +214,10 @@ gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
|
||||
.software_bias_gain = 1.0 / 32.0,
|
||||
.software_settle_updates = 16u
|
||||
};
|
||||
break;
|
||||
case GPS_PPS_PROFILE_HOLDOVER:
|
||||
default:
|
||||
return (gps_pps_loop_profile_t){
|
||||
parameters = (gps_pps_loop_profile_t){
|
||||
.natural_frequency_hz = 0.0,
|
||||
.damping = 1.0,
|
||||
.max_slew_ppm = 20.0,
|
||||
@@ -174,7 +227,106 @@ gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
|
||||
.software_bias_gain = 1.0 / 32.0,
|
||||
.software_settle_updates = 16u
|
||||
};
|
||||
break;
|
||||
}
|
||||
parameters.lock_acquire_updates = gps_pps_period_updates(
|
||||
parameters.lock_acquire_updates, update_period_sec,
|
||||
GPS_PPS_MIN_LOCK_UPDATES);
|
||||
parameters.software_settle_updates = gps_pps_period_updates(
|
||||
parameters.software_settle_updates, update_period_sec,
|
||||
GPS_PPS_MIN_SETTLE_UPDATES);
|
||||
return parameters;
|
||||
}
|
||||
|
||||
bool gps_pps_loop_coefficients(
|
||||
gps_pps_loop_profile_t parameters,
|
||||
double update_period_sec,
|
||||
double clock_frequency_hz,
|
||||
int64_t* kp_q46_out,
|
||||
int64_t* ki_q46_out,
|
||||
uint32_t* max_slew_q32_out)
|
||||
{
|
||||
if((kp_q46_out == NULL) || (ki_q46_out == NULL) ||
|
||||
(max_slew_q32_out == NULL) ||
|
||||
!isfinite(update_period_sec) || (update_period_sec <= 0.0) ||
|
||||
!isfinite(clock_frequency_hz) || (clock_frequency_hz <= 0.0) ||
|
||||
!isfinite(parameters.natural_frequency_hz) ||
|
||||
(parameters.natural_frequency_hz < 0.0) ||
|
||||
!isfinite(parameters.damping) || (parameters.damping < 0.0) ||
|
||||
!isfinite(parameters.max_slew_ppm) ||
|
||||
(parameters.max_slew_ppm < 0.0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
const double pi = 3.14159265358979323846;
|
||||
const double q46 = 70368744177664.0;
|
||||
const double q32 = 4294967296.0;
|
||||
const double baseline_sec = GPS_PPS_BASELINE_UPDATE_PERIOD_SEC;
|
||||
const double wn = 2.0 * pi * parameters.natural_frequency_hz;
|
||||
/* Use the actual deployed Q2.46 baseline, not its ideal double value, so
|
||||
* the mapped poles are those of the proven fixed-point implementation. */
|
||||
const double baseline_kp =
|
||||
(double)llround(
|
||||
(2.0 * parameters.damping * wn / clock_frequency_hz) * q46) /
|
||||
q46;
|
||||
const double baseline_ki =
|
||||
(double)llround(
|
||||
(wn * wn * baseline_sec / clock_frequency_hz) * q46) /
|
||||
q46;
|
||||
const double baseline_ticks = clock_frequency_hz * baseline_sec;
|
||||
const double baseline_sum =
|
||||
2.0 - baseline_ticks * (baseline_kp + baseline_ki);
|
||||
const double baseline_product =
|
||||
1.0 - baseline_ticks * baseline_kp;
|
||||
const double discriminant =
|
||||
baseline_sum * baseline_sum - 4.0 * baseline_product;
|
||||
const double period_ratio = update_period_sec / baseline_sec;
|
||||
double target_sum = 0.0;
|
||||
double target_product = 0.0;
|
||||
|
||||
if(discriminant >= 0.0)
|
||||
{
|
||||
double root = sqrt(discriminant);
|
||||
double pole_a = 0.5 * (baseline_sum + root);
|
||||
double pole_b = 0.5 * (baseline_sum - root);
|
||||
if((pole_a < 0.0) || (pole_b < 0.0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double target_a = pow(pole_a, period_ratio);
|
||||
double target_b = pow(pole_b, period_ratio);
|
||||
target_sum = target_a + target_b;
|
||||
target_product = target_a * target_b;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(baseline_product <= 0.0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double radius = sqrt(baseline_product);
|
||||
double cosine = baseline_sum / (2.0 * radius);
|
||||
cosine = fmax(-1.0, fmin(1.0, cosine));
|
||||
double angle = acos(cosine);
|
||||
double target_radius = pow(radius, period_ratio);
|
||||
target_sum =
|
||||
2.0 * target_radius * cos(angle * period_ratio);
|
||||
target_product = target_radius * target_radius;
|
||||
}
|
||||
|
||||
double target_ticks = clock_frequency_hz * update_period_sec;
|
||||
double kp = (1.0 - target_product) / target_ticks;
|
||||
double ki = (1.0 - target_sum + target_product) / target_ticks;
|
||||
if(!isfinite(kp) || !isfinite(ki) || (kp < 0.0) || (ki < 0.0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
*kp_q46_out = (int64_t)llround(kp * q46);
|
||||
*ki_q46_out = (int64_t)llround(ki * q46);
|
||||
*max_slew_q32_out = (uint32_t)llround(
|
||||
parameters.max_slew_ppm * 1.0e-6 * q32);
|
||||
return true;
|
||||
}
|
||||
|
||||
const char* gps_pps_profile_name(gps_pps_profile_t profile)
|
||||
|
||||
@@ -20,6 +20,7 @@
|
||||
#define GPS_PPS_FILTER_INNOVATION_CLAMP_M 300.0
|
||||
#define GPS_PPS_FILTER_HARD_OUTLIER_M 1500.0
|
||||
#define GPS_PPS_SPEED_OF_LIGHT_MPS 299792458.0
|
||||
#define GPS_PPS_FILTER_BASELINE_PERIOD_SEC 0.200
|
||||
|
||||
static double clamp_double(double value, double limit)
|
||||
{
|
||||
@@ -34,6 +35,21 @@ static double clamp_double(double value, double limit)
|
||||
return value;
|
||||
}
|
||||
|
||||
/* Preserve the baseline discrete pole when the navigation interval changes.
|
||||
* Applying a baseline gain alpha N times is equivalent to one elapsed-time
|
||||
* update with alpha_T = 1 - (1 - alpha)^N. This also handles an occasional
|
||||
* missed solve without slowing the clock-bias or drift estimator. */
|
||||
static double gps_pps_filter_elapsed_gain(
|
||||
double baseline_gain, double elapsed_sec)
|
||||
{
|
||||
double periods = elapsed_sec / GPS_PPS_FILTER_BASELINE_PERIOD_SEC;
|
||||
if(fabs(periods - 1.0) <= 1.0e-12)
|
||||
{
|
||||
return baseline_gain;
|
||||
}
|
||||
return -expm1(periods * log1p(-baseline_gain));
|
||||
}
|
||||
|
||||
void gps_pps_time_filter_init(gps_pps_time_filter_t* filter)
|
||||
{
|
||||
if(filter != NULL)
|
||||
@@ -269,14 +285,20 @@ gps_pps_time_filter_result_t gps_pps_time_filter_update(
|
||||
return GPS_PPS_TIME_FILTER_REJECTED;
|
||||
}
|
||||
|
||||
double effective_bias_gain =
|
||||
gps_pps_filter_elapsed_gain(bias_gain, elapsed_sec);
|
||||
double effective_drift_gain = gps_pps_filter_elapsed_gain(
|
||||
GPS_PPS_FILTER_DRIFT_GAIN, elapsed_sec);
|
||||
double effective_fallback_drift_gain = gps_pps_filter_elapsed_gain(
|
||||
GPS_PPS_FILTER_FALLBACK_DRIFT_GAIN, elapsed_sec);
|
||||
filter->clock_bias_m =
|
||||
predicted_clock_bias_m +
|
||||
bias_gain *
|
||||
effective_bias_gain *
|
||||
clamp_double(innovation_m, GPS_PPS_FILTER_INNOVATION_CLAMP_M);
|
||||
if(clock_drift_valid)
|
||||
{
|
||||
filter->clock_drift_mps +=
|
||||
GPS_PPS_FILTER_DRIFT_GAIN *
|
||||
effective_drift_gain *
|
||||
(measured_clock_drift_mps - filter->clock_drift_mps);
|
||||
}
|
||||
else
|
||||
@@ -291,7 +313,7 @@ gps_pps_time_filter_result_t gps_pps_time_filter_update(
|
||||
else
|
||||
{
|
||||
filter->clock_drift_mps +=
|
||||
GPS_PPS_FILTER_FALLBACK_DRIFT_GAIN *
|
||||
effective_fallback_drift_gain *
|
||||
(fallback_drift_mps - filter->clock_drift_mps);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -42,3 +42,13 @@ uint64_t gps_receiver_status_ticks_per_second(void)
|
||||
{
|
||||
return GPS_SYSTICK_HZ;
|
||||
}
|
||||
|
||||
void gps_receiver_set_eskf_doppler_alpha(double alpha)
|
||||
{
|
||||
gps_tracking_set_eskf_doppler_alpha(alpha);
|
||||
}
|
||||
|
||||
double gps_receiver_get_eskf_doppler_alpha(void)
|
||||
{
|
||||
return gps_tracking_get_eskf_doppler_alpha();
|
||||
}
|
||||
|
||||
@@ -8,6 +8,8 @@
|
||||
#define GPS_TRACK_MEASUREMENT_MIN_SATELLITES 4u
|
||||
#define GPS_TRACK_MEASUREMENT_MAX_SATELLITES GPS_TRCK_IP_COUNT
|
||||
#define GPS_TRACK_MEASUREMENT_EPSILON 1.0e-12
|
||||
#define GPS_TRACK_VELOCITY_FDE_MIN_SATELLITES 6u
|
||||
#define GPS_TRACK_VELOCITY_FDE_SIGMA_GATE 3.5
|
||||
|
||||
static double clamp_double(double value, double minimum, double maximum)
|
||||
{
|
||||
@@ -135,6 +137,14 @@ bool gps_tracking_measurement_read(
|
||||
measurement_out->pseudorange_rate_mps =
|
||||
gps_tracking_doppler_to_range_rate_mps(
|
||||
measurement_out->doppler_hz);
|
||||
measurement_out->filtered_doppler_hz =
|
||||
measurement_out->doppler_hz;
|
||||
measurement_out->filtered_pseudorange_rate_mps =
|
||||
measurement_out->pseudorange_rate_mps;
|
||||
measurement_out->eskf_doppler_hz =
|
||||
measurement_out->doppler_hz;
|
||||
measurement_out->eskf_pseudorange_rate_mps =
|
||||
measurement_out->pseudorange_rate_mps;
|
||||
|
||||
unsigned int version =
|
||||
meta >> GPS_TRCK_MEAS_META_VERSION_SHIFT;
|
||||
@@ -149,7 +159,11 @@ bool gps_tracking_measurement_read(
|
||||
(measurement_out->cn0_sample_count == 20u) &&
|
||||
cn0_valid &&
|
||||
isfinite(measurement_out->doppler_hz) &&
|
||||
isfinite(measurement_out->pseudorange_rate_mps);
|
||||
isfinite(measurement_out->pseudorange_rate_mps) &&
|
||||
isfinite(measurement_out->eskf_doppler_hz) &&
|
||||
isfinite(measurement_out->eskf_pseudorange_rate_mps) &&
|
||||
isfinite(measurement_out->filtered_doppler_hz) &&
|
||||
isfinite(measurement_out->filtered_pseudorange_rate_mps);
|
||||
return measurement_out->valid;
|
||||
}
|
||||
|
||||
@@ -261,7 +275,94 @@ static bool invert_four_by_four(double input[4][4], double inverse[4][4])
|
||||
return true;
|
||||
}
|
||||
|
||||
bool gps_tracking_velocity_wls(
|
||||
typedef struct {
|
||||
double solution[4];
|
||||
double normal_inverse[4][4];
|
||||
double residual_mps[GPS_TRACK_MEASUREMENT_MAX_SATELLITES];
|
||||
double residual_square_sum;
|
||||
double weighted_residual_square_sum;
|
||||
unsigned int row_count;
|
||||
} gps_velocity_wls_candidate_t;
|
||||
|
||||
static bool gps_velocity_solve_candidate(
|
||||
const double design[][4],
|
||||
const double observation[],
|
||||
const double weights[],
|
||||
unsigned int used,
|
||||
unsigned int excluded_row,
|
||||
gps_velocity_wls_candidate_t* candidate_out)
|
||||
{
|
||||
if((design == NULL) || (observation == NULL) || (weights == NULL) ||
|
||||
(candidate_out == NULL) ||
|
||||
(used < GPS_TRACK_MEASUREMENT_MIN_SATELLITES) ||
|
||||
(used > GPS_TRACK_MEASUREMENT_MAX_SATELLITES))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double normal[4][4] = {{0.0}};
|
||||
double rhs[4] = {0.0};
|
||||
unsigned int selected = 0u;
|
||||
for(unsigned int measurement_row = 0u;
|
||||
measurement_row < used;
|
||||
measurement_row++)
|
||||
{
|
||||
if(measurement_row == excluded_row)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
double weight = weights[measurement_row];
|
||||
for(unsigned int row = 0u; row < 4u; row++)
|
||||
{
|
||||
rhs[row] += weight * design[measurement_row][row] *
|
||||
observation[measurement_row];
|
||||
for(unsigned int column = 0u; column < 4u; column++)
|
||||
{
|
||||
normal[row][column] +=
|
||||
weight * design[measurement_row][row] *
|
||||
design[measurement_row][column];
|
||||
}
|
||||
}
|
||||
selected++;
|
||||
}
|
||||
if(selected < GPS_TRACK_MEASUREMENT_MIN_SATELLITES)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
memset(candidate_out, 0, sizeof(*candidate_out));
|
||||
if(!solve_four_by_four(normal, rhs, candidate_out->solution) ||
|
||||
!invert_four_by_four(normal, candidate_out->normal_inverse))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for(unsigned int measurement_row = 0u;
|
||||
measurement_row < used;
|
||||
measurement_row++)
|
||||
{
|
||||
candidate_out->residual_mps[measurement_row] = NAN;
|
||||
if(measurement_row == excluded_row)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
double predicted = 0.0;
|
||||
for(unsigned int column = 0u; column < 4u; column++)
|
||||
{
|
||||
predicted += design[measurement_row][column] *
|
||||
candidate_out->solution[column];
|
||||
}
|
||||
double residual = observation[measurement_row] - predicted;
|
||||
candidate_out->residual_mps[measurement_row] = residual;
|
||||
candidate_out->residual_square_sum += residual * residual;
|
||||
candidate_out->weighted_residual_square_sum +=
|
||||
weights[measurement_row] * residual * residual;
|
||||
}
|
||||
candidate_out->row_count = selected;
|
||||
return isfinite(candidate_out->residual_square_sum) &&
|
||||
isfinite(candidate_out->weighted_residual_square_sum);
|
||||
}
|
||||
|
||||
static bool gps_tracking_velocity_wls_internal(
|
||||
const double receiver_ecef_m[3],
|
||||
const double satellite_positions_m[][3],
|
||||
const double satellite_velocities_mps[][3],
|
||||
@@ -271,8 +372,14 @@ bool gps_tracking_velocity_wls(
|
||||
double receiver_velocity_ecef_mps_out[3],
|
||||
double receiver_velocity_covariance_ecef_mps2_out[3][3],
|
||||
double* receiver_clock_drift_mps_out,
|
||||
double* residual_rms_mps_out)
|
||||
double* residual_rms_mps_out,
|
||||
bool robust,
|
||||
uint32_t* excluded_measurement_mask_out)
|
||||
{
|
||||
if(excluded_measurement_mask_out != NULL)
|
||||
{
|
||||
*excluded_measurement_mask_out = 0u;
|
||||
}
|
||||
if((receiver_ecef_m == NULL) || (satellite_positions_m == NULL) ||
|
||||
(satellite_velocities_mps == NULL) ||
|
||||
(satellite_clock_drifts_sps == NULL) ||
|
||||
@@ -287,11 +394,10 @@ bool gps_tracking_velocity_wls(
|
||||
return false;
|
||||
}
|
||||
|
||||
double normal[4][4] = {{0.0}};
|
||||
double rhs[4] = {0.0};
|
||||
double design[GPS_TRACK_MEASUREMENT_MAX_SATELLITES][4];
|
||||
double observation[GPS_TRACK_MEASUREMENT_MAX_SATELLITES];
|
||||
double weights[GPS_TRACK_MEASUREMENT_MAX_SATELLITES];
|
||||
unsigned int measurement_index[GPS_TRACK_MEASUREMENT_MAX_SATELLITES];
|
||||
unsigned int used = 0u;
|
||||
|
||||
for(unsigned int satellite = 0u; satellite < count; satellite++)
|
||||
@@ -342,17 +448,7 @@ bool gps_tracking_velocity_wls(
|
||||
sigma_rate_mps = clamp_double(sigma_rate_mps, 0.05, 5.0);
|
||||
double weight = 1.0 / (sigma_rate_mps * sigma_rate_mps);
|
||||
weights[used] = weight;
|
||||
for(unsigned int row = 0u; row < 4u; row++)
|
||||
{
|
||||
rhs[row] +=
|
||||
weight * design[used][row] * observation[used];
|
||||
for(unsigned int column = 0u; column < 4u; column++)
|
||||
{
|
||||
normal[row][column] +=
|
||||
weight * design[used][row] *
|
||||
design[used][column];
|
||||
}
|
||||
}
|
||||
measurement_index[used] = satellite;
|
||||
used++;
|
||||
}
|
||||
|
||||
@@ -361,27 +457,47 @@ bool gps_tracking_velocity_wls(
|
||||
return false;
|
||||
}
|
||||
|
||||
double solution[4];
|
||||
double normal_inverse[4][4];
|
||||
if(!solve_four_by_four(normal, rhs, solution) ||
|
||||
!invert_four_by_four(normal, normal_inverse))
|
||||
gps_velocity_wls_candidate_t full;
|
||||
if(!gps_velocity_solve_candidate(
|
||||
design, observation, weights, used,
|
||||
GPS_TRACK_MEASUREMENT_MAX_SATELLITES, &full))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double residual_square_sum = 0.0;
|
||||
double weighted_residual_square_sum = 0.0;
|
||||
for(unsigned int row = 0u; row < used; row++)
|
||||
gps_velocity_wls_candidate_t selected = full;
|
||||
unsigned int excluded_row = GPS_TRACK_MEASUREMENT_MAX_SATELLITES;
|
||||
if(robust && (used >= GPS_TRACK_VELOCITY_FDE_MIN_SATELLITES))
|
||||
{
|
||||
double predicted = 0.0;
|
||||
for(unsigned int column = 0u; column < 4u; column++)
|
||||
double maximum_normalized_residual = 0.0;
|
||||
for(unsigned int row = 0u; row < used; row++)
|
||||
{
|
||||
predicted += design[row][column] * solution[column];
|
||||
double normalized =
|
||||
fabs(full.residual_mps[row]) * sqrt(weights[row]);
|
||||
maximum_normalized_residual =
|
||||
fmax(maximum_normalized_residual, normalized);
|
||||
}
|
||||
double residual = observation[row] - predicted;
|
||||
residual_square_sum += residual * residual;
|
||||
weighted_residual_square_sum +=
|
||||
weights[row] * residual * residual;
|
||||
if(maximum_normalized_residual > GPS_TRACK_VELOCITY_FDE_SIGMA_GATE)
|
||||
{
|
||||
double best_score = INFINITY;
|
||||
for(unsigned int trial_row = 0u; trial_row < used; trial_row++)
|
||||
{
|
||||
gps_velocity_wls_candidate_t trial;
|
||||
if(gps_velocity_solve_candidate(
|
||||
design, observation, weights, used, trial_row, &trial) &&
|
||||
(trial.weighted_residual_square_sum < best_score))
|
||||
{
|
||||
best_score = trial.weighted_residual_square_sum;
|
||||
selected = trial;
|
||||
excluded_row = trial_row;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if((excluded_row < used) && (excluded_measurement_mask_out != NULL))
|
||||
{
|
||||
*excluded_measurement_mask_out =
|
||||
1u << measurement_index[excluded_row];
|
||||
}
|
||||
|
||||
/*
|
||||
@@ -392,17 +508,19 @@ bool gps_tracking_velocity_wls(
|
||||
* the poor geometry appear directly in the inverse normal matrix.
|
||||
*/
|
||||
double covariance_scale = 1.0;
|
||||
if(used > 4u)
|
||||
if(selected.row_count > 4u)
|
||||
{
|
||||
covariance_scale = fmax(
|
||||
1.0, weighted_residual_square_sum / (double)(used - 4u));
|
||||
1.0,
|
||||
selected.weighted_residual_square_sum /
|
||||
(double)(selected.row_count - 4u));
|
||||
}
|
||||
for(unsigned int row = 0u; row < 3u; row++)
|
||||
{
|
||||
for(unsigned int column = 0u; column < 3u; column++)
|
||||
{
|
||||
receiver_velocity_covariance_ecef_mps2_out[row][column] =
|
||||
covariance_scale * normal_inverse[row][column];
|
||||
covariance_scale * selected.normal_inverse[row][column];
|
||||
if(!isfinite(
|
||||
receiver_velocity_covariance_ecef_mps2_out[row][column]))
|
||||
{
|
||||
@@ -410,10 +528,53 @@ bool gps_tracking_velocity_wls(
|
||||
}
|
||||
}
|
||||
}
|
||||
memcpy(receiver_velocity_ecef_mps_out, solution,
|
||||
3u * sizeof(solution[0]));
|
||||
*receiver_clock_drift_mps_out = solution[3];
|
||||
memcpy(receiver_velocity_ecef_mps_out, selected.solution,
|
||||
3u * sizeof(selected.solution[0]));
|
||||
*receiver_clock_drift_mps_out = selected.solution[3];
|
||||
*residual_rms_mps_out =
|
||||
sqrt(residual_square_sum / (double)used);
|
||||
sqrt(selected.residual_square_sum / (double)selected.row_count);
|
||||
return isfinite(*residual_rms_mps_out);
|
||||
}
|
||||
|
||||
bool gps_tracking_velocity_wls(
|
||||
const double receiver_ecef_m[3],
|
||||
const double satellite_positions_m[][3],
|
||||
const double satellite_velocities_mps[][3],
|
||||
const double satellite_clock_drifts_sps[],
|
||||
const gps_tracking_measurement_t measurements[],
|
||||
unsigned int count,
|
||||
double receiver_velocity_ecef_mps_out[3],
|
||||
double receiver_velocity_covariance_ecef_mps2_out[3][3],
|
||||
double* receiver_clock_drift_mps_out,
|
||||
double* residual_rms_mps_out)
|
||||
{
|
||||
return gps_tracking_velocity_wls_internal(
|
||||
receiver_ecef_m, satellite_positions_m,
|
||||
satellite_velocities_mps, satellite_clock_drifts_sps,
|
||||
measurements, count, receiver_velocity_ecef_mps_out,
|
||||
receiver_velocity_covariance_ecef_mps2_out,
|
||||
receiver_clock_drift_mps_out, residual_rms_mps_out,
|
||||
false, NULL);
|
||||
}
|
||||
|
||||
bool gps_tracking_velocity_wls_robust(
|
||||
const double receiver_ecef_m[3],
|
||||
const double satellite_positions_m[][3],
|
||||
const double satellite_velocities_mps[][3],
|
||||
const double satellite_clock_drifts_sps[],
|
||||
const gps_tracking_measurement_t measurements[],
|
||||
unsigned int count,
|
||||
double receiver_velocity_ecef_mps_out[3],
|
||||
double receiver_velocity_covariance_ecef_mps2_out[3][3],
|
||||
double* receiver_clock_drift_mps_out,
|
||||
double* residual_rms_mps_out,
|
||||
uint32_t* excluded_measurement_mask_out)
|
||||
{
|
||||
return gps_tracking_velocity_wls_internal(
|
||||
receiver_ecef_m, satellite_positions_m,
|
||||
satellite_velocities_mps, satellite_clock_drifts_sps,
|
||||
measurements, count, receiver_velocity_ecef_mps_out,
|
||||
receiver_velocity_covariance_ecef_mps2_out,
|
||||
receiver_clock_drift_mps_out, residual_rms_mps_out,
|
||||
true, excluded_measurement_mask_out);
|
||||
}
|
||||
|
||||
@@ -91,14 +91,11 @@ static void profile_coefficients(
|
||||
int64_t* ki_q46,
|
||||
int64_t* max_slew_q32)
|
||||
{
|
||||
const double pi = 3.14159265358979323846;
|
||||
double wn = 2.0 * pi * profile.natural_frequency_hz;
|
||||
*kp_q46 = (int64_t)llround(
|
||||
(2.0 * profile.damping * wn / TRACKING_HZ) * Q46_SCALE);
|
||||
*ki_q46 = (int64_t)llround(
|
||||
(wn * wn * UPDATE_PERIOD_SEC / TRACKING_HZ) * Q46_SCALE);
|
||||
*max_slew_q32 = (int64_t)llround(
|
||||
profile.max_slew_ppm * 1.0e-6 * Q32_SCALE);
|
||||
uint32_t max_slew = 0u;
|
||||
assert(gps_pps_loop_coefficients(
|
||||
profile, UPDATE_PERIOD_SEC, TRACKING_HZ,
|
||||
kp_q46, ki_q46, &max_slew));
|
||||
*max_slew_q32 = max_slew;
|
||||
}
|
||||
|
||||
static void synthetic_rtl_coarse(synthetic_rtl_t* rtl, int64_t reference_q32)
|
||||
|
||||
@@ -107,6 +107,69 @@ static void test_profile_parameters_are_ordered(void)
|
||||
assert(robust.lock_release_ticks >= precision.lock_release_ticks);
|
||||
}
|
||||
|
||||
static void test_period_scaled_hysteresis_and_settling(void)
|
||||
{
|
||||
gps_pps_quality_selector_t selector;
|
||||
gps_pps_quality_selector_init_period(&selector, 0.5);
|
||||
assert(selector.upgrade_updates == 4u);
|
||||
assert(selector.degrade_updates == 2u);
|
||||
gps_pps_loop_profile_t precision =
|
||||
gps_pps_loop_profile_for_period(GPS_PPS_PROFILE_PRECISION, 0.5);
|
||||
gps_pps_loop_profile_t robust =
|
||||
gps_pps_loop_profile_for_period(GPS_PPS_PROFILE_ROBUST, 0.5);
|
||||
assert(precision.lock_acquire_updates == 2u);
|
||||
assert(precision.software_settle_updates == 5u);
|
||||
assert(robust.lock_acquire_updates == 4u);
|
||||
assert(robust.software_settle_updates == 7u);
|
||||
|
||||
gps_pps_quality_selector_init_period(&selector, 1.0);
|
||||
assert(selector.upgrade_updates == 2u);
|
||||
assert(selector.degrade_updates == 2u);
|
||||
precision = gps_pps_loop_profile_for_period(
|
||||
GPS_PPS_PROFILE_PRECISION, 1.0);
|
||||
robust = gps_pps_loop_profile_for_period(
|
||||
GPS_PPS_PROFILE_ROBUST, 1.0);
|
||||
assert(precision.lock_acquire_updates == 2u);
|
||||
assert(precision.software_settle_updates == 3u);
|
||||
assert(robust.lock_acquire_updates == 2u);
|
||||
assert(robust.software_settle_updates == 4u);
|
||||
}
|
||||
|
||||
static void assert_coefficients(
|
||||
gps_pps_profile_t profile,
|
||||
double period_sec,
|
||||
int64_t expected_kp,
|
||||
int64_t expected_ki)
|
||||
{
|
||||
int64_t kp = 0;
|
||||
int64_t ki = 0;
|
||||
uint32_t max_slew = 0u;
|
||||
gps_pps_loop_profile_t parameters =
|
||||
gps_pps_loop_profile_for_period(profile, period_sec);
|
||||
assert(gps_pps_loop_coefficients(
|
||||
parameters, period_sec, 125000000.0,
|
||||
&kp, &ki, &max_slew));
|
||||
assert(kp == expected_kp);
|
||||
assert(ki == expected_ki);
|
||||
assert(max_slew == 85899u);
|
||||
}
|
||||
|
||||
static void test_period_scaled_discrete_coefficients(void)
|
||||
{
|
||||
/* The production 200 ms coefficients remain bit-for-bit unchanged. */
|
||||
assert_coefficients(GPS_PPS_PROFILE_ACQUIRE, 0.2, 764018, 64006);
|
||||
assert_coefficients(GPS_PPS_PROFILE_PRECISION, 0.2, 222838, 5445);
|
||||
assert_coefficients(GPS_PPS_PROFILE_ROBUST, 0.2, 84891, 640);
|
||||
|
||||
/* Other cadences exponentiate those proven discrete poles in time. */
|
||||
assert_coefficients(GPS_PPS_PROFILE_ACQUIRE, 0.5, 615780, 127492);
|
||||
assert_coefficients(GPS_PPS_PROFILE_PRECISION, 0.5, 209783, 12804);
|
||||
assert_coefficients(GPS_PPS_PROFILE_ROBUST, 0.5, 82980, 1564);
|
||||
assert_coefficients(GPS_PPS_PROFILE_ACQUIRE, 1.0, 447388, 178038);
|
||||
assert_coefficients(GPS_PPS_PROFILE_PRECISION, 1.0, 190239, 23149);
|
||||
assert_coefficients(GPS_PPS_PROFILE_ROBUST, 1.0, 79923, 3011);
|
||||
}
|
||||
|
||||
static void test_quality_boundaries_and_recovery(void)
|
||||
{
|
||||
gps_pps_quality_selector_t selector;
|
||||
@@ -167,6 +230,8 @@ int main(void)
|
||||
test_hysteretic_profile_transitions();
|
||||
test_alternating_quality_does_not_upgrade();
|
||||
test_profile_parameters_are_ordered();
|
||||
test_period_scaled_hysteresis_and_settling();
|
||||
test_period_scaled_discrete_coefficients();
|
||||
test_quality_boundaries_and_recovery();
|
||||
puts("gps_pps_quality tests passed");
|
||||
return 0;
|
||||
|
||||
@@ -6,6 +6,30 @@
|
||||
|
||||
#define COUNTER_HZ 125000000.0
|
||||
|
||||
static void test_elapsed_time_gain_preserves_filter_poles(void)
|
||||
{
|
||||
gps_pps_time_filter_t filter;
|
||||
gps_pps_time_filter_init(&filter);
|
||||
double filtered = 0.0;
|
||||
double innovation = 0.0;
|
||||
|
||||
assert(gps_pps_time_filter_update(
|
||||
&filter, 0u, COUNTER_HZ, 0.0, true, 0.0, 1.0,
|
||||
1.0 / 32.0, 1u, &filtered, &innovation) ==
|
||||
GPS_PPS_TIME_FILTER_COARSE);
|
||||
assert(gps_pps_time_filter_update(
|
||||
&filter, (uint64_t)COUNTER_HZ, COUNTER_HZ, 100.0, true, 100.0,
|
||||
1.0, 1.0 / 32.0, 1u, &filtered, &innovation) ==
|
||||
GPS_PPS_TIME_FILTER_TRACKING);
|
||||
|
||||
/* One 1 s update has exactly the pole of five baseline 200 ms updates. */
|
||||
double expected_bias_gain = 1.0 - pow(1.0 - 1.0 / 32.0, 5.0);
|
||||
double expected_drift_gain = 1.0 - pow(1.0 - 1.0 / 4.0, 5.0);
|
||||
assert(fabs(filtered - 100.0 * expected_bias_gain) < 1.0e-12);
|
||||
assert(fabs(filter.clock_drift_mps - 100.0 * expected_drift_gain) <
|
||||
1.0e-12);
|
||||
}
|
||||
|
||||
static void test_drift_prediction_rejects_bias_noise(void)
|
||||
{
|
||||
static const double noise_m[] = {
|
||||
@@ -138,6 +162,99 @@ static void test_bias_ramp_without_doppler_uses_bounded_fallback(void)
|
||||
assert(sqrt(squared_error_sum / (double)tracked) < 45.0);
|
||||
}
|
||||
|
||||
static void test_board_calibrated_doppler_predictor_beats_fallback(void)
|
||||
{
|
||||
/* Values measured on Genesys2 on 2026-07-26: clock-bias sigma was
|
||||
* 15.87 m and reference-datum drift was about 291.75 m/s. Feed the
|
||||
* exact same deterministic WLS noise to two filters so this is a true
|
||||
* predictor on/off A/B, not two unrelated Monte-Carlo runs. */
|
||||
static const double bias_noise_m[] = {
|
||||
8.0, -18.0, 12.0, -7.0, 21.0, -14.0, 5.0, -25.0,
|
||||
16.0, -3.0, 10.0, -20.0, 6.0, 24.0, -11.0, 2.0
|
||||
};
|
||||
static const double drift_noise_mps[] = {
|
||||
1.8, -3.2, 4.1, -2.4, 0.7, 3.5, -4.3, 2.6,
|
||||
-1.1, 4.6, -2.8, 1.5, -3.7, 2.2, -0.4, 3.0
|
||||
};
|
||||
gps_pps_time_filter_t aided;
|
||||
gps_pps_time_filter_t fallback;
|
||||
gps_pps_time_filter_init(&aided);
|
||||
gps_pps_time_filter_init(&fallback);
|
||||
|
||||
double aided_squared_error_sum = 0.0;
|
||||
double fallback_squared_error_sum = 0.0;
|
||||
unsigned int tracked = 0u;
|
||||
for(unsigned int update = 0u; update < 900u; update++)
|
||||
{
|
||||
double time_sec = 0.2 * (double)update;
|
||||
double long_omega = 2.0 * 3.14159265358979323846 / 75.0;
|
||||
double short_omega = 2.0 * 3.14159265358979323846 / 19.0;
|
||||
double varying_bias_m =
|
||||
5.0 * (1.0 - cos(long_omega * time_sec)) / long_omega +
|
||||
2.0 * (1.0 - cos(short_omega * time_sec)) / short_omega;
|
||||
double true_drift_mps =
|
||||
291.75 + 5.0 * sin(long_omega * time_sec) +
|
||||
2.0 * sin(short_omega * time_sec);
|
||||
double true_bias_m =
|
||||
159624.654 + 291.75 * time_sec + varying_bias_m;
|
||||
double measured_bias_m = true_bias_m +
|
||||
bias_noise_m[update %
|
||||
(sizeof(bias_noise_m) / sizeof(bias_noise_m[0]))];
|
||||
double aided_bias_m = 0.0;
|
||||
double fallback_bias_m = 0.0;
|
||||
double innovation_m = 0.0;
|
||||
gps_pps_time_filter_result_t aided_result =
|
||||
gps_pps_time_filter_update(
|
||||
&aided,
|
||||
(uint64_t)llround(time_sec * COUNTER_HZ),
|
||||
COUNTER_HZ,
|
||||
measured_bias_m,
|
||||
true,
|
||||
true_drift_mps + drift_noise_mps[update %
|
||||
(sizeof(drift_noise_mps) /
|
||||
sizeof(drift_noise_mps[0]))],
|
||||
1.5,
|
||||
1.0 / 32.0,
|
||||
12u,
|
||||
&aided_bias_m,
|
||||
&innovation_m);
|
||||
gps_pps_time_filter_result_t fallback_result =
|
||||
gps_pps_time_filter_update(
|
||||
&fallback,
|
||||
(uint64_t)llround(time_sec * COUNTER_HZ),
|
||||
COUNTER_HZ,
|
||||
measured_bias_m,
|
||||
false,
|
||||
0.0,
|
||||
1.5,
|
||||
1.0 / 32.0,
|
||||
12u,
|
||||
&fallback_bias_m,
|
||||
&innovation_m);
|
||||
assert(aided_result != GPS_PPS_TIME_FILTER_REJECTED);
|
||||
assert(fallback_result != GPS_PPS_TIME_FILTER_REJECTED);
|
||||
if((update >= 300u) &&
|
||||
(aided_result == GPS_PPS_TIME_FILTER_TRACKING) &&
|
||||
(fallback_result == GPS_PPS_TIME_FILTER_TRACKING))
|
||||
{
|
||||
double aided_error_m = aided_bias_m - true_bias_m;
|
||||
double fallback_error_m = fallback_bias_m - true_bias_m;
|
||||
aided_squared_error_sum += aided_error_m * aided_error_m;
|
||||
fallback_squared_error_sum +=
|
||||
fallback_error_m * fallback_error_m;
|
||||
tracked++;
|
||||
}
|
||||
}
|
||||
assert(tracked > 550u);
|
||||
double aided_rms_m = sqrt(aided_squared_error_sum / (double)tracked);
|
||||
double fallback_rms_m =
|
||||
sqrt(fallback_squared_error_sum / (double)tracked);
|
||||
printf("board PPS bias RMS: Doppler=%.3f m fallback=%.3f m\n",
|
||||
aided_rms_m, fallback_rms_m);
|
||||
assert(aided_rms_m < 8.0);
|
||||
assert(aided_rms_m < 0.35 * fallback_rms_m);
|
||||
}
|
||||
|
||||
static void test_reference_geometry_defines_bias_drift(void)
|
||||
{
|
||||
const double receiver_position_m[3] = {0.0, 0.0, 0.0};
|
||||
@@ -205,9 +322,11 @@ static void test_reference_selection_is_stable(void)
|
||||
|
||||
int main(void)
|
||||
{
|
||||
test_elapsed_time_gain_preserves_filter_poles();
|
||||
test_drift_prediction_rejects_bias_noise();
|
||||
test_quality_gap_and_outlier_handling();
|
||||
test_bias_ramp_without_doppler_uses_bounded_fallback();
|
||||
test_board_calibrated_doppler_predictor_beats_fallback();
|
||||
test_reference_geometry_defines_bias_drift();
|
||||
test_reference_selection_is_stable();
|
||||
puts("gps_pps_time_filter tests passed");
|
||||
|
||||
@@ -78,6 +78,22 @@ static void test_cn0_and_mailbox_decode(void)
|
||||
measurement.pseudorange_rate_mps,
|
||||
expected_range_rate,
|
||||
1.0e-9));
|
||||
assert(close_to(
|
||||
measurement.filtered_doppler_hz,
|
||||
measurement.doppler_hz,
|
||||
1.0e-12));
|
||||
assert(close_to(
|
||||
measurement.filtered_pseudorange_rate_mps,
|
||||
measurement.pseudorange_rate_mps,
|
||||
1.0e-12));
|
||||
assert(close_to(
|
||||
measurement.eskf_doppler_hz,
|
||||
measurement.doppler_hz,
|
||||
1.0e-12));
|
||||
assert(close_to(
|
||||
measurement.eskf_pseudorange_rate_mps,
|
||||
measurement.pseudorange_rate_mps,
|
||||
1.0e-12));
|
||||
|
||||
registers.MEASUREMENT_META &=
|
||||
~(1u << GPS_TRCK_MEAS_META_LOCK_POS);
|
||||
@@ -180,6 +196,86 @@ static void test_velocity_wls(void)
|
||||
solved_velocity_covariance[0][1],
|
||||
solved_velocity_covariance[1][0],
|
||||
1.0e-12));
|
||||
{
|
||||
double clean_velocity[3];
|
||||
double clean_covariance[3][3];
|
||||
double clean_clock_drift;
|
||||
double clean_residual_rms;
|
||||
uint32_t clean_excluded_mask = UINT32_MAX;
|
||||
assert(gps_tracking_velocity_wls_robust(
|
||||
receiver_position,
|
||||
satellite_positions,
|
||||
satellite_velocities,
|
||||
satellite_clock_drift,
|
||||
measurements,
|
||||
TEST_SATELLITES,
|
||||
clean_velocity,
|
||||
clean_covariance,
|
||||
&clean_clock_drift,
|
||||
&clean_residual_rms,
|
||||
&clean_excluded_mask));
|
||||
assert(clean_excluded_mask == 0u);
|
||||
for(unsigned int component = 0u; component < 3u; component++)
|
||||
{
|
||||
assert(close_to(
|
||||
clean_velocity[component],
|
||||
solved_velocity[component],
|
||||
1.0e-12));
|
||||
}
|
||||
}
|
||||
|
||||
/* One gross carrier burst must be isolated before aggregate ESKF fusion. */
|
||||
measurements[2].pseudorange_rate_mps += 12.0;
|
||||
double plain_velocity[3];
|
||||
double plain_covariance[3][3];
|
||||
double plain_clock_drift;
|
||||
double plain_residual_rms;
|
||||
assert(gps_tracking_velocity_wls(
|
||||
receiver_position,
|
||||
satellite_positions,
|
||||
satellite_velocities,
|
||||
satellite_clock_drift,
|
||||
measurements,
|
||||
TEST_SATELLITES,
|
||||
plain_velocity,
|
||||
plain_covariance,
|
||||
&plain_clock_drift,
|
||||
&plain_residual_rms));
|
||||
double robust_velocity[3];
|
||||
double robust_covariance[3][3];
|
||||
double robust_clock_drift;
|
||||
double robust_residual_rms;
|
||||
uint32_t excluded_mask = 0u;
|
||||
assert(gps_tracking_velocity_wls_robust(
|
||||
receiver_position,
|
||||
satellite_positions,
|
||||
satellite_velocities,
|
||||
satellite_clock_drift,
|
||||
measurements,
|
||||
TEST_SATELLITES,
|
||||
robust_velocity,
|
||||
robust_covariance,
|
||||
&robust_clock_drift,
|
||||
&robust_residual_rms,
|
||||
&excluded_mask));
|
||||
assert(excluded_mask == (1u << 2));
|
||||
double plain_error_square = 0.0;
|
||||
for(unsigned int component = 0u; component < 3u; component++)
|
||||
{
|
||||
double plain_error =
|
||||
plain_velocity[component] - expected_velocity[component];
|
||||
plain_error_square += plain_error * plain_error;
|
||||
assert(close_to(
|
||||
robust_velocity[component],
|
||||
expected_velocity[component],
|
||||
1.0e-8));
|
||||
}
|
||||
assert(sqrt(plain_error_square) > 1.0);
|
||||
assert(close_to(
|
||||
robust_clock_drift,
|
||||
expected_clock_drift_mps,
|
||||
1.0e-8));
|
||||
assert(robust_residual_rms < 1.0e-8);
|
||||
}
|
||||
|
||||
int main(void)
|
||||
|
||||
@@ -0,0 +1,260 @@
|
||||
# GPS PPS end-to-end Linux validation
|
||||
|
||||
This package is a deterministic, causal validation of the project timing path
|
||||
from synthetic GPS LNAV data to the physical PPS edge. It exists separately
|
||||
from `pps_python`, whose models concentrate on WLS noise, oscillator behavior,
|
||||
and adaptive-loop performance.
|
||||
|
||||
The validation does not use Linux wall-clock scheduling. The 1 kHz, 50 bit/s,
|
||||
RF-sample, PCLK, and 125 MHz events advance from synthetic timestamps, making
|
||||
the signed absolute-time result repeatable on a normal Linux host.
|
||||
|
||||
## What is modeled
|
||||
|
||||
The model follows the current firmware and RTL conventions:
|
||||
|
||||
1. It generates deterministic GPS LNAV subframes with D29*/D30* data inversion
|
||||
and valid parity for all ten 30-bit words.
|
||||
2. Each navigation bit is expanded to twenty 1 kHz prompt-sign samples.
|
||||
3. The `bit_gen.sv` 40 ms `1 -> 0` transition acquisition, majority decision,
|
||||
20 ms `bit_valid` cadence, and registered ABS_SAMPLE behavior are applied.
|
||||
4. The tracking wrapper's chronological `2 + 300 + 8 = 310` bit window,
|
||||
preamble search, parity check, `load_sigs`, and subframe interrupt ordering
|
||||
are applied.
|
||||
5. `nav_solve_timer` is initialized from HOW Z-count and counts later bit-valid
|
||||
events. Navigation-solve interrupt, PPS counter capture, capture sequence,
|
||||
and a 32-bit RF ABS_SAMPLE wrap are exercised.
|
||||
6. Firmware reconstructs periodic TOW from the HOW/sample anchor and evaluates
|
||||
`TOW + POSTPONE_TOW + STARTOFFSET - b/c`.
|
||||
7. The timestamped 125 MHz PPS peripheral performs coarse/normal mapping,
|
||||
ignores synthetic CPU/APB delay as phase, acquires lock after five valid
|
||||
updates, and generates integer-second edges.
|
||||
8. DUT edges are compared with an independent trusted PPS. Optional DUT and
|
||||
trusted-output delays make board, cable, and vendor delay sign explicit.
|
||||
|
||||
This is a digital epoch-path validation, not an RF waveform simulator. The
|
||||
existing `pps_python` tests continue to cover WLS, oscillator, PI, adaptive
|
||||
quality, and holdover behavior.
|
||||
|
||||
## Run
|
||||
|
||||
From `soc/software`:
|
||||
|
||||
```bash
|
||||
python3 -m pps_end_to_end_validation.pps_end_to_end
|
||||
python3 -m unittest pps_end_to_end_validation/test_pps_end_to_end.py
|
||||
```
|
||||
|
||||
The default command executes the diagnostic matrix:
|
||||
|
||||
```text
|
||||
scenario bits correction reference error DUT - trusted PPS
|
||||
current-rtl/180ms 9 180 ms -0.000032 ms +0.000032 ms
|
||||
current-rtl/160ms 9 160 ms -20.000032 ms +20.000032 ms
|
||||
current-bit/160ms 8 160 ms -0.000032 ms +0.000032 ms
|
||||
```
|
||||
|
||||
Positive `DUT - trusted PPS` means the project output is late. The default
|
||||
32 ns remainder is the explicitly configured four-tick PCLK capture pipeline;
|
||||
it is not part of the LNAV-bit mismatch.
|
||||
|
||||
Run one scenario or obtain machine-readable output:
|
||||
|
||||
```bash
|
||||
python3 -m pps_end_to_end_validation.pps_end_to_end \
|
||||
--mode current-rtl --postpone-ms 160 --json
|
||||
|
||||
python3 -m pps_end_to_end_validation.pps_end_to_end \
|
||||
--mode current-bit \
|
||||
--dut-output-delay-ns 12 \
|
||||
--trusted-output-delay-ns 35
|
||||
```
|
||||
|
||||
## Closed-loop stability and dynamic tuning
|
||||
|
||||
The bit-level command above isolates the absolute epoch convention. The
|
||||
companion stability analysis connects that result to the fixed-point
|
||||
firmware/RTL PI model, replays the real GDOP and tracked-satellite history in
|
||||
`fusion/logs/gps_live_5min_20260713.csv`, and synthesizes only the telemetry
|
||||
that the repository did not capture:
|
||||
|
||||
```bash
|
||||
python3 -m pps_end_to_end_validation.stability_analysis
|
||||
python3 -m pps_end_to_end_validation.stability_analysis \
|
||||
--update-period-ms 500
|
||||
python3 -m pps_end_to_end_validation.stability_analysis \
|
||||
--update-period-ms 1000
|
||||
python3 -m unittest \
|
||||
pps_end_to_end_validation/test_stability_analysis.py
|
||||
```
|
||||
|
||||
Generated results are in `output/stability/`:
|
||||
|
||||
- `stability_report.md`: real-data inventory, discrete poles, absolute phase
|
||||
wander, period jitter, dynamic parameter sweep, and recommendations;
|
||||
- `parameter_sweep.csv`: machine-readable results for every scenario/tuning;
|
||||
- `synthetic_telemetry.csv`: solve-cadence ABS_SAMPLE, REM_CODE_PHASE, corrected 125 MHz
|
||||
capture, HDOP/VDOP/TDOP/GDOP, C/N0, pseudorange noise, clock sigma/drift,
|
||||
oscillator and timing-error inputs.
|
||||
|
||||
The older fusion capture used by `stability_analysis.py` contains 989 GDOP
|
||||
values and 990 tracked-satellite values (eight throughout), but no PPS
|
||||
phase/rate, ABS_SAMPLE, C/N0, TDOP/HDOP, pseudorange residual, oscillator
|
||||
temperature, or trusted-edge/TIC samples. The newer live-board workflow below
|
||||
supplies many of those PPS status fields, while ABS_SAMPLE, REM_CODE_PHASE,
|
||||
temperature, and trusted-edge timing remain unavailable. The stability CSV
|
||||
labels replacements as synthetic; it must not be read as board telemetry.
|
||||
|
||||
The current acquire, precision, and robust fixed-point poles are inside the
|
||||
unit circle at the 200, 500, and 1000 ms supported update rates, and the stress
|
||||
analysis observes no unlock. It also exposes an important metrics issue:
|
||||
cycle-to-cycle PPS period
|
||||
jitter can remain below 20 ns while absolute phase moves slowly through tens
|
||||
of nanoseconds. A physical 20 ns maximum therefore requires a common-clock
|
||||
time-interval-counter capture and cable/output-delay calibration; a p95 period
|
||||
jitter assertion or synthetic run cannot certify it.
|
||||
|
||||
The sweep includes the current coefficients, quieter/faster precision loops,
|
||||
a robust-noise guard, a larger Doppler-drift gain, and a quality-scheduled
|
||||
candidate. The generated report contains the current numerical comparison.
|
||||
Production firmware maps the proven 200 ms discrete poles to the selected NAV
|
||||
period and leaves the 200 ms Q2.46 constants bit-identical. The model and
|
||||
firmware also scale quality/lock/settling timers in wall-clock time, convert
|
||||
software bias/drift gains from actual elapsed time, and extend the stale-data
|
||||
watchdog to `max(2 s, 3 * solve period)`. This preserves deterministic settling
|
||||
and damping; it cannot preserve measurement-noise statistics when fewer WLS
|
||||
samples are available. Production firmware uses the current-RTL-compatible
|
||||
180 ms epoch correction independently of solve cadence.
|
||||
|
||||
A deterministic four-seed, 180 s smoke sweep with the current profiles gave:
|
||||
|
||||
| solve period | real-geometry absolute RMS | worst period step over current scenarios | unlocks |
|
||||
|---:|---:|---:|---:|
|
||||
| 200 ms | 11.17 ns | 16 ns | 0 |
|
||||
| 500 ms | 15.96 ns | 16 ns | 0 |
|
||||
| 1000 ms | 25.09 ns | 24 ns | 0 |
|
||||
|
||||
Thus 1000 ms is linearly stable but is not recommended when the stated
|
||||
synthetic maximum-period-jitter target is 20 ns. Use 200 ms by default, or
|
||||
500 ms when CPU load requires it. A 1000 ms solve needs hardware validation
|
||||
and likely oscillator-temperature/frequency prediction; pole remapping alone
|
||||
cannot recreate the missing measurements.
|
||||
|
||||
## Live board telemetry replay
|
||||
|
||||
`hardware_replay.py` accepts the host-timestamped UART CSV produced by the
|
||||
fusion capture utility. It extracts the firmware fields that actually drive
|
||||
or describe PPS: raw/filtered WLS clock bias, innovation, Doppler datum drift,
|
||||
TDOP/GDOP/HDOP/VDOP, clock sigma, C/N0, tracked/used satellites, velocity/FDE,
|
||||
reference SVID, fractional capture correction, RTL phase residual, rate word,
|
||||
integrator, lock/profile, pulse count, and commit sequence.
|
||||
|
||||
Capture and replay from `soc/software`:
|
||||
|
||||
```bash
|
||||
python3 fusion/serial_capture.py \
|
||||
--port /dev/ttyUSB2 --baud 460800 --duration 300 \
|
||||
--output /tmp/genesys2_pps.csv
|
||||
|
||||
python3 -m pps_end_to_end_validation.hardware_replay \
|
||||
--input /tmp/genesys2_pps.csv \
|
||||
--output-dir pps_end_to_end_validation/output/hardware_replay
|
||||
|
||||
python3 -m unittest \
|
||||
pps_end_to_end_validation.test_hardware_replay
|
||||
```
|
||||
|
||||
Raw UART files are ignored by Git because `[LOCATION]` records contain exact
|
||||
coordinates. The replay output is normalized to PPS fields and deliberately
|
||||
omits latitude/longitude.
|
||||
|
||||
For an already-running board, replay imports the first logged locked
|
||||
filter/PI snapshot. Starting the model from reset would erase the observed
|
||||
integrator and create a false acquisition transient. When status is decimated,
|
||||
commit sequence numbers expand the trace back to the inferred solve cadence;
|
||||
intermediate values are explicitly marked `interpolated=true`.
|
||||
|
||||
The replay infers cadence from UART host time divided by commit-sequence
|
||||
advance, snapped to the 20 ms navigation-bit grid. Use
|
||||
`--update-period-ms` only when host timestamps are unreliable.
|
||||
|
||||
The report separates three quantities that should not be conflated:
|
||||
|
||||
- direct `hw_phase` statistics from the board;
|
||||
- fixed-point PI reproduction and a telemetry-conditioned model replay; and
|
||||
- inferred active phase obtained by integrating `rate_cmd` under a constant
|
||||
oscillator assumption.
|
||||
|
||||
Only the first two are directly constrained by firmware/RTL telemetry. The
|
||||
third is useful for diagnosing slow wander but is not a trusted-generator
|
||||
measurement. `hw_phase` itself is a 5 Hz commit residual, not the physical PPS
|
||||
pin offset. A calibrated, common-clock TIC/scope capture is still required for
|
||||
a maximum-20 ns output claim.
|
||||
|
||||
## 20 ms diagnosis
|
||||
|
||||
The current tracking wrapper shifts the current bit into `shift_reg_n`, while
|
||||
its preamble/parity predicate reads the preceding `shift_reg_p`. On the eighth
|
||||
following preamble bit, the validator therefore sees only the previous seven
|
||||
bits. The register contains all eight after the edge, but the bit-valid edge
|
||||
condition has ended. Validation occurs on the ninth bit, 20 ms later.
|
||||
|
||||
The implemented interface is consequently:
|
||||
|
||||
```text
|
||||
current RTL delivery 9 * 20 ms = 180 ms
|
||||
production POSTPONE_TOW 9 * 20 ms = 180 ms
|
||||
signed epoch mismatch 0 ms
|
||||
|
||||
former firmware value 8 * 20 ms = 160 ms
|
||||
former signed mismatch 20 ms
|
||||
```
|
||||
|
||||
Production firmware now uses `POSTPONE_TOW=0.180` as the current-RTL-compatible
|
||||
correction, consistently in both satellite-state and PPS calculations. It is
|
||||
180 ms, not 190 ms. If RTL is later changed to validate a candidate window
|
||||
containing the current eighth bit, firmware must return to 160 ms in the same
|
||||
coordinated release.
|
||||
|
||||
The validation intentionally does not modify production RTL. Firmware changes
|
||||
for period-aware control are exercised by the same coefficient and timing
|
||||
tests; the 180 ms absolute epoch convention remains unchanged.
|
||||
|
||||
## Other regression coverage
|
||||
|
||||
The unit tests verify:
|
||||
|
||||
- parity-valid LNAV round-trip and rejection after one corrupted data bit;
|
||||
- 1 kHz prompt-sign to 50 bit/s generation and the registered 1 ms sample lag;
|
||||
- ninth-bit delivery in current RTL and eighth-bit delivery in the candidate
|
||||
implementation;
|
||||
- constant signed PPS error over successive physical edges;
|
||||
- 180 ms firmware compatibility with current RTL;
|
||||
- invariance to synthetic CPU/APB stalls when capture timestamps are used;
|
||||
- DUT-versus-trusted output-delay sign; and
|
||||
- rejection behavior explaining why firmware now compile-rejects
|
||||
`NAV_SOLVE_TIMER=40`; and
|
||||
- zero NAV-solve rejection and unchanged absolute epoch at N=10/25/50
|
||||
(200/500/1000 ms).
|
||||
|
||||
The `N=40` case divides the GPS week but not a 300-bit subframe. When the model
|
||||
refreshes the HOW/sample anchor after six seconds, the timer phase is half a
|
||||
solve period away from that anchor and the current `N * 20 ms` snap rejects
|
||||
otherwise valid solves. Firmware therefore requires N to divide both the GPS
|
||||
week and the 300-bit subframe. Production-safe examples are N=10, 25, and 50.
|
||||
|
||||
If arbitrary timer periods are required later, reconstruct elapsed time on the
|
||||
fundamental 20 ms navigation-bit grid instead of snapping through a refreshed
|
||||
HOW anchor.
|
||||
|
||||
## Hardware confirmation
|
||||
|
||||
Before selecting a production correction:
|
||||
|
||||
1. Probe `bit_valid`, the final preamble bits, `preamble_ok`, parity result, and
|
||||
`load_sigs` with ILA.
|
||||
2. Count whether delivery is on following bit 8 or 9 in the flashed bitstream.
|
||||
3. Measure DUT and trusted PPS with a common time-interval counter and record
|
||||
the sign. The diagnosis predicts a DUT edge approximately 20 ms late.
|
||||
4. Apply only nanosecond-scale output/cable calibration after the epoch
|
||||
mismatch is removed.
|
||||
@@ -0,0 +1 @@
|
||||
"""End-to-end Linux validation for the GPS LNAV-to-PPS timing path."""
|
||||
File diff suppressed because it is too large
Load Diff
+150
@@ -0,0 +1,150 @@
|
||||
host_monotonic_s,host_unix_s,time_s,profile,mode,locked,tdop,gdop,hdop,vdop,clock_sigma_m,cn0_mean_dbhz,cn0_min_dbhz,tracked_satellites,used_satellites,innovation_m,raw_bias_m,filtered_bias_m,clock_drift_mps,bias_drift_mps,predictor_enabled,eskf_alpha,eskf_speed_mps,eskf_velocity_rms_mps,eskf_fde_mask,pps_speed_mps,pps_velocity_rms_mps,pps_fde_mask,reference_svid,capture_correction_ns,hardware_phase_ns,rate_command_ppm,integrator_ppm,pulse_count,sequence,status
|
||||
1.50420269,1785272055.903764,0.0,robust,tracking,True,1.25,2.86,1.8176601213,1.8176260703,12.54,42.7,37.7,6,5,4.52,693498.817,693494.442,110.788,-72.055,True,0.05,0.271,0.185,0,0.271,0.185,0,23,32.0,160.47,-1.536,-1.56,600,3040,2063
|
||||
3.503689524,1785272057.9032507,2.0,robust,tracking,True,1.25,2.86,1.8176193953,1.8170996688,11.17,42.9,38.2,6,5,-19.2,693330.906,693349.504,111.201,-71.738,True,0.05,0.746,0.135,0,0.746,0.135,0,23,56.0,124.38,-1.541,-1.56,602,3050,2063
|
||||
5.500479302,1785272059.9000404,4.0,robust,tracking,True,1.25,2.86,1.8175780522,1.8165687846,21.48,42.7,38.5,6,5,-7.0,693197.92,693204.698,111.206,-71.912,True,0.05,0.836,0.039,0,0.836,0.039,0,23,24.0,141.5,-1.539,-1.56,604,3060,2063
|
||||
7.509988539,1785272061.9095497,6.0,robust,tracking,True,1.25,2.86,1.8175381286,1.816041859,11.14,43.0,38.3,6,5,13.6,693072.274,693059.102,111.221,-71.825,True,0.05,1.009,0.28,0,1.009,0.28,0,23,32.0,168.11,-1.535,-1.56,606,3070,2063
|
||||
9.511653187,1785272063.9112144,8.0,robust,tracking,True,1.25,2.86,1.8174962624,1.8155120506,11.56,42.9,38.0,6,5,-31.15,692881.989,692912.165,110.864,-72.861,True,0.05,0.371,0.04,0,0.371,0.04,0,23,40.0,135.63,-1.539,-1.56,608,3080,2063
|
||||
11.50378733,1785272065.9033487,10.0,robust,tracking,True,1.25,2.85,1.8174539102,1.8149874373,11.07,42.8,38.6,6,5,-9.78,692754.373,692763.848,110.014,-73.882,True,0.05,0.843,0.004,0,0.843,0.004,0,23,56.0,155.42,-1.536,-1.56,610,3090,2063
|
||||
13.504178648,1785272067.9037397,12.0,robust,tracking,True,1.25,2.85,1.817413463,1.8144599125,11.18,42.9,38.1,6,5,6.48,692623.758,692617.477,110.329,-73.94,True,0.05,0.609,0.096,0,0.609,0.096,0,23,0.0,162.66,-1.535,-1.56,612,3100,2063
|
||||
15.503554083,1785272069.9031153,14.0,robust,tracking,True,1.25,2.85,1.8173718635,1.8139319104,11.26,42.7,37.0,6,5,-1.31,692469.561,692470.834,111.54,-72.849,True,0.05,0.953,0.31,0,0.953,0.31,0,23,56.0,155.78,-1.536,-1.56,614,3110,2063
|
||||
17.502924749,1785272071.9024858,16.0,robust,tracking,True,1.25,2.85,1.81733102,1.8134069522,15.4,42.4,36.5,6,5,-11.0,692313.987,692324.644,110.815,-73.581,True,0.05,0.736,0.168,0,0.736,0.168,0,23,32.0,133.05,-1.54,-1.56,616,3120,2063
|
||||
19.506236288,1785272073.9057975,18.0,robust,tracking,True,1.24,2.85,1.8172902798,1.8128821788,11.11,43.1,38.1,6,5,-5.77,692171.911,692177.505,111.077,-73.779,True,0.05,0.4,0.027,0,0.4,0.027,0,23,24.0,131.86,-1.54,-1.56,618,3130,2063
|
||||
21.504228726,1785272075.9037895,20.0,robust,tracking,True,1.24,2.85,1.8172490099,1.8123554135,11.15,43.0,38.0,6,5,8.57,692038.349,692030.045,110.612,-74.424,True,0.05,0.512,0.053,0,0.512,0.053,0,23,24.0,156.87,-1.536,-1.56,620,3140,2063
|
||||
23.504153615,1785272077.903715,22.0,robust,tracking,True,1.24,2.85,1.8172104392,1.8118290291,11.3,42.6,37.7,6,5,-11.79,691868.479,691879.901,111.534,-73.806,True,0.05,0.721,0.036,0,0.721,0.036,0,23,56.0,142.52,-1.538,-1.56,622,3150,2063
|
||||
25.510000609,1785272079.9095619,24.0,robust,tracking,True,1.24,2.85,1.8171689199,1.8113038519,11.08,43.1,38.1,6,5,-12.44,691719.345,691731.399,110.942,-74.863,True,0.05,0.359,0.101,0,0.359,0.101,0,23,48.0,145.5,-1.538,-1.56,624,3160,2063
|
||||
27.50517151,1785272081.904733,26.0,robust,tracking,True,1.24,2.85,1.8171301373,1.8107796086,11.26,42.7,37.5,6,5,-18.16,691564.145,691581.742,111.5,-74.479,True,0.05,0.745,0.096,0,0.745,0.096,0,23,8.0,129.02,-1.54,-1.56,626,3170,2063
|
||||
29.502641745,1785272083.9022028,28.0,robust,tracking,True,1.24,2.85,1.8170880804,1.8102547189,12.41,43.0,37.5,6,5,1.28,691433.115,691431.876,111.148,-74.903,True,0.05,0.332,0.115,0,0.332,0.115,0,23,0.0,127.84,-1.541,-1.56,628,3180,2063
|
||||
31.500642933,1785272085.900204,30.0,robust,tracking,True,1.24,2.85,1.8170480561,1.8097326484,14.78,42.6,37.5,6,5,-1.39,691280.067,691281.411,111.681,-74.941,True,0.05,0.503,0.255,0,0.503,0.255,0,23,48.0,147.24,-1.538,-1.56,630,3190,2063
|
||||
33.504307045,1785272087.903868,32.0,robust,tracking,True,1.24,2.85,1.8170082113,1.8092081529,22.56,42.9,38.4,6,5,-13.17,691116.408,691129.168,110.696,-75.765,True,0.05,0.206,0.027,0,0.206,0.027,0,23,16.0,132.81,-1.54,-1.56,632,3200,2063
|
||||
35.509887989,1785272089.9094489,34.0,robust,tracking,True,1.24,2.85,1.8169688875,1.8086870549,11.18,42.9,38.0,6,5,-0.84,690974.741,690975.554,111.343,-75.376,True,0.05,0.636,0.055,0,0.636,0.055,0,23,56.0,156.39,-1.536,-1.56,634,3210,2063
|
||||
37.506370892,1785272091.9059322,36.0,robust,tracking,True,1.24,2.85,1.8169289218,1.8081636846,10.95,43.4,39.5,6,5,-3.76,690819.152,690822.79,110.566,-76.732,True,0.05,0.72,0.058,0,0.72,0.058,0,23,32.0,156.77,-1.536,-1.56,636,3220,2063
|
||||
39.504571912,1785272093.904133,38.0,robust,tracking,True,1.24,2.85,1.8168882102,1.8076411645,11.05,43.1,38.3,6,5,-8.88,690659.561,690668.166,111.385,-76.264,True,0.05,0.403,0.084,0,0.403,0.084,0,23,48.0,147.43,-1.538,-1.56,638,3230,2063
|
||||
41.502657038,1785272095.9022183,40.0,robust,tracking,True,1.24,2.85,1.8168471874,1.8071184407,11.13,42.7,38.1,6,5,5.56,690520.399,690515.009,111.68,-75.754,True,0.05,0.957,0.102,0,0.957,0.102,0,23,32.0,147.4,-1.538,-1.56,640,3240,2063
|
||||
43.50032369,1785272097.8998854,42.0,robust,tracking,True,1.24,2.85,1.816809289,1.8065980348,11.21,42.7,37.8,6,5,9.98,690370.621,690360.952,111.268,-76.081,True,0.05,0.807,0.041,0,0.807,0.041,0,23,0.0,136.35,-1.539,-1.56,642,3250,2063
|
||||
45.712022504,1785272100.111584,44.0,robust,tracking,True,1.24,2.85,1.8167660234,1.8060226878,12.45,43.2,38.0,6,5,-7.85,690184.849,690192.456,111.097,-76.306,True,0.05,0.794,0.037,0,0.794,0.037,0,23,32.0,142.31,-1.538,-1.56,645,3260,2063
|
||||
47.704670721,1785272102.1042318,46.0,robust,tracking,True,1.24,2.85,1.8167259917,1.8055025786,11.59,42.9,38.7,6,5,-14.42,690023.356,690037.324,110.386,-77.903,True,0.05,0.84,0.099,0,0.84,0.099,0,23,56.0,136.57,-1.539,-1.56,647,3270,2063
|
||||
49.704359089,1785272104.1039205,48.0,robust,tracking,True,1.24,2.84,1.8166840843,1.804981981,11.04,43.4,38.2,6,5,-2.28,689880.716,689882.925,110.905,-78.019,True,0.05,0.509,0.107,0,0.509,0.107,0,23,16.0,148.6,-1.537,-1.56,649,3280,2063
|
||||
51.709139916,1785272106.1087012,50.0,robust,tracking,True,1.24,2.84,1.8166456009,1.8044613748,10.99,43.3,38.1,6,5,-0.68,689726.579,689727.233,111.064,-77.798,True,0.05,0.094,0.103,0,0.094,0.103,0,23,48.0,144.29,-1.538,-1.56,651,3290,2063
|
||||
53.703365655,1785272108.1029265,52.0,robust,tracking,True,1.24,2.84,1.8166083592,1.8039440331,12.68,42.8,37.5,6,5,-8.72,689564.342,689572.793,110.533,-78.338,True,0.05,0.504,0.187,0,0.504,0.187,0,23,8.0,148.54,-1.537,-1.56,653,3300,2063
|
||||
55.700643307,1785272110.100204,54.0,robust,tracking,True,1.24,2.84,1.8165672875,1.8034221236,11.07,42.9,38.7,6,5,4.12,689421.398,689417.403,111.098,-77.598,True,0.05,0.645,0.08,0,0.645,0.08,0,23,40.0,140.91,-1.539,-1.56,655,3310,2063
|
||||
57.904055806,1785272112.3036168,56.0,robust,tracking,True,1.24,2.84,1.8165247981,1.8028524558,16.67,42.9,38.2,6,5,-1.04,689243.202,689244.211,110.99,-78.254,True,0.05,0.342,0.02,0,0.342,0.02,0,23,48.0,144.27,-1.538,-1.56,657,3320,2063
|
||||
60.10451733,1785272114.5040786,58.0,robust,tracking,True,1.24,2.84,1.8164827704,1.8022817921,11.19,42.9,38.0,6,5,-5.64,689066.66,689072.123,111.676,-78.014,True,0.05,0.73,0.15,0,0.73,0.15,0,23,48.0,126.86,-1.541,-1.56,659,3330,2063
|
||||
62.108759952,1785272116.5083213,60.0,robust,tracking,True,1.24,2.84,1.8164435096,1.8017644864,16.38,43.1,39.2,6,5,-1.09,688913.112,688914.171,111.17,-78.316,True,0.05,0.696,0.007,0,0.696,0.007,0,23,24.0,153.74,-1.537,-1.56,661,3340,2063
|
||||
64.300493137,1785272118.7000537,62.0,robust,tracking,True,1.24,2.84,1.8163984438,1.8011921234,11.32,42.4,37.4,6,5,8.12,688749.473,688741.606,110.452,-79.51,True,0.05,0.546,0.16,0,0.546,0.16,0,23,40.0,152.97,-1.537,-1.56,663,3350,2063
|
||||
66.510932531,1785272120.9104934,64.0,robust,tracking,True,1.23,2.84,1.8163566675,1.8006252849,20.15,42.7,38.3,6,5,-2.84,688563.016,688565.769,110.92,-78.813,True,0.05,0.795,0.197,0,0.795,0.197,0,23,40.0,172.22,-1.535,-1.561,665,3360,2063
|
||||
68.704372014,1785272123.103933,66.0,robust,tracking,True,1.23,2.84,1.8163145194,1.8000555234,15.52,42.8,38.5,6,5,-11.85,688380.019,688391.495,110.061,-80.47,True,0.05,0.796,0.074,0,0.796,0.074,0,23,48.0,155.17,-1.537,-1.56,668,3370,2063
|
||||
70.703104898,1785272125.102666,68.0,robust,tracking,True,1.23,2.84,1.8162769265,1.7995395272,11.43,42.0,37.8,6,5,13.98,688244.124,688230.58,110.609,-80.264,True,0.05,0.381,0.125,0,0.381,0.125,0,23,16.0,179.45,-1.533,-1.56,670,3380,2063
|
||||
72.702223062,1785272127.101784,70.0,robust,tracking,True,1.23,2.84,1.8162378878,1.7990227767,16.58,42.8,38.9,6,5,5.88,688078.671,688072.971,110.939,-80.36,True,0.05,0.666,0.053,0,0.666,0.053,0,23,24.0,166.76,-1.535,-1.56,672,3390,2063
|
||||
74.700286036,1785272129.0998468,72.0,robust,tracking,True,1.23,2.84,1.816199267,1.7985063001,11.1,42.7,38.7,6,5,14.57,687926.671,687912.557,110.527,-80.516,True,0.05,0.312,0.03,0,0.312,0.03,0,23,32.0,162.81,-1.536,-1.56,674,3400,2063
|
||||
76.71281947,1785272131.1123805,74.0,robust,tracking,True,1.23,2.84,1.8161600375,1.7979893506,11.11,42.7,38.6,6,5,-1.67,687750.089,687751.702,111.576,-79.6,True,0.05,1.026,0.083,0,1.026,0.083,0,23,16.0,142.71,-1.539,-1.56,676,3410,2063
|
||||
78.704243128,1785272133.103804,76.0,robust,tracking,True,1.23,2.84,1.8161215432,1.7974747243,11.08,43.2,38.1,6,5,-4.48,687586.716,687591.059,110.819,-81.044,True,0.05,0.676,0.104,0,0.676,0.104,0,23,0.0,107.47,-1.544,-1.56,678,3420,2063
|
||||
80.70401895,1785272135.10358,78.0,robust,tracking,True,1.23,2.84,1.8160831759,1.7969606981,20.64,42.3,37.5,6,5,-0.58,687429.447,687430.01,110.975,-81.135,True,0.05,0.351,0.106,0,0.351,0.106,0,23,56.0,161.62,-1.536,-1.56,680,3430,2063
|
||||
82.710133903,1785272137.1096952,80.0,robust,tracking,True,1.23,2.84,1.8160471938,1.7964462492,21.18,43.0,39.1,6,5,11.46,687279.439,687268.334,110.976,-80.409,True,0.05,1.15,0.031,0,1.15,0.031,0,23,0.0,177.64,-1.534,-1.56,682,3440,2063
|
||||
84.702669179,1785272139.1022303,82.0,robust,tracking,True,1.23,2.84,1.8160084174,1.7959315494,16.16,42.7,39.0,6,5,2.07,687108.261,687106.252,110.31,-81.912,True,0.05,0.401,0.04,0,0.401,0.04,0,23,48.0,145.45,-1.539,-1.56,684,3450,2063
|
||||
86.700732144,1785272141.100293,84.0,robust,tracking,True,1.23,2.83,1.8159713938,1.7954167669,11.13,42.7,39.0,6,5,-12.66,686929.679,686941.939,110.952,-81.183,True,0.05,0.726,0.196,0,0.726,0.196,0,23,24.0,139.84,-1.539,-1.56,686,3460,2063
|
||||
88.704090128,1785272143.103651,86.0,robust,tracking,True,1.23,2.83,1.8159335443,1.79490275,16.21,42.7,38.7,6,5,8.63,686784.906,686776.542,110.694,-82.024,True,0.05,0.209,0.052,0,0.209,0.052,0,23,48.0,176.01,-1.534,-1.56,688,3470,2063
|
||||
90.704048278,1785272145.103609,88.0,robust,tracking,True,1.23,2.83,1.8158959585,1.7943905432,24.91,42.5,37.9,6,5,-0.15,686613.039,686613.185,110.563,-82.135,True,0.05,0.379,0.123,0,0.379,0.123,0,23,40.0,161.5,-1.536,-1.56,690,3480,2063
|
||||
92.711508414,1785272147.1110694,90.0,robust,tracking,True,1.23,2.83,1.8158570425,1.793877413,24.43,42.7,38.0,6,5,15.4,686465.242,686450.326,111.554,-81.637,True,0.05,1.011,0.184,0,1.011,0.184,0,23,56.0,178.15,-1.534,-1.56,692,3490,2063
|
||||
94.704749369,1785272149.1043103,92.0,robust,tracking,True,1.23,2.83,1.8158190794,1.7933647389,21.48,42.4,38.5,6,5,16.84,686303.4,686287.088,110.534,-82.726,True,0.05,0.195,0.029,0,0.195,0.029,0,23,48.0,171.95,-1.535,-1.56,694,3500,2063
|
||||
96.703104991,1785272151.1026661,94.0,robust,tracking,True,1.23,2.83,1.815781917,1.7928524573,11.31,42.4,37.6,6,5,-1.05,686119.874,686120.892,109.749,-83.836,True,0.05,0.919,0.158,0,0.919,0.158,0,23,16.0,137.68,-1.54,-1.56,696,3510,2063
|
||||
98.915123597,1785272153.3146844,96.0,robust,tracking,True,1.23,2.83,1.8157417458,1.792292301,11.49,42.2,37.0,6,5,-4.81,685933.786,685938.441,110.18,-83.485,True,0.05,0.687,0.188,0,0.687,0.188,0,23,40.0,130.24,-1.541,-1.56,698,3520,2063
|
||||
101.104184426,1785272155.5037456,98.0,robust,tracking,True,1.23,2.83,1.8157012539,1.7917269932,14.98,42.4,37.7,6,5,-4.93,685751.271,685756.046,110.623,-83.514,True,0.05,0.285,0.185,0,0.285,0.185,0,23,40.0,144.59,-1.539,-1.56,700,3530,2063
|
||||
103.108802454,1785272157.5083635,100.0,robust,tracking,True,1.23,2.83,1.8156626947,1.7912163488,25.23,42.4,38.0,6,5,8.11,685596.218,685588.366,110.401,-83.817,True,0.05,0.362,0.053,0,0.362,0.053,0,23,56.0,172.28,-1.535,-1.56,702,3540,2063
|
||||
105.105464086,1785272159.505025,102.0,robust,tracking,True,1.23,2.83,1.8156280022,1.79070636,16.51,42.5,37.5,6,5,-15.09,685403.733,685418.356,110.792,-83.601,True,0.05,0.365,0.1,0,0.365,0.1,0,23,48.0,134.38,-1.54,-1.56,704,3550,2063
|
||||
107.300312593,1785272161.6998734,104.0,robust,tracking,True,1.23,2.83,1.8155869207,1.790143438,20.41,42.6,38.3,6,5,4.06,685236.104,685232.167,111.771,-83.119,True,0.05,1.06,0.172,0,1.06,0.172,0,23,56.0,186.11,-1.532,-1.56,706,3560,2063
|
||||
109.312626871,1785272163.7121878,106.0,robust,tracking,True,1.23,2.83,1.8155513007,1.7896363278,15.58,42.6,37.5,6,5,-5.9,685055.343,685061.062,110.302,-84.628,True,0.05,0.319,0.208,0,0.319,0.208,0,23,24.0,168.36,-1.535,-1.56,708,3570,2063
|
||||
111.306453913,1785272165.7060149,108.0,robust,tracking,True,1.23,2.83,1.8155128487,1.789126556,21.63,42.6,38.0,6,5,16.48,684906.901,684890.938,110.502,-84.943,True,0.05,0.385,0.093,0,0.385,0.093,0,23,16.0,177.8,-1.534,-1.56,710,3580,2063
|
||||
113.305323246,1785272167.7048843,110.0,robust,tracking,True,1.22,2.83,1.8154776515,1.7886176858,11.1,42.9,38.4,6,5,1.51,684721.855,684720.397,110.396,-84.747,True,0.05,0.197,0.218,0,0.197,0.218,0,23,32.0,163.81,-1.536,-1.56,712,3590,2063
|
||||
115.30411121,1785272169.7036724,112.0,robust,tracking,True,1.22,2.83,1.8154394251,1.7881082833,22.64,43.1,38.0,6,5,-3.4,684547.463,684550.752,110.44,-85.241,True,0.05,0.429,0.029,0,0.429,0.029,0,23,8.0,152.36,-1.538,-1.56,714,3600,2063
|
||||
117.303101565,1785272171.702663,114.0,robust,tracking,True,1.22,2.83,1.8154034756,1.7875997643,11.52,42.6,38.1,6,5,17.65,684398.444,684381.346,110.707,-84.967,True,0.05,0.405,0.059,0,0.405,0.059,0,23,24.0,192.86,-1.532,-1.56,716,3610,2063
|
||||
119.306150448,1785272173.7057114,116.0,robust,tracking,True,1.22,2.83,1.8153676614,1.7870927007,12.04,42.5,37.5,6,5,-7.99,684203.354,684211.094,110.164,-85.459,True,0.05,0.631,0.155,0,0.631,0.155,0,23,32.0,144.3,-1.539,-1.56,718,3620,2063
|
||||
121.300962576,1785272175.7005236,118.0,robust,tracking,True,1.22,2.83,1.8153303586,1.786584068,18.38,42.4,37.2,6,5,15.28,684055.675,684040.872,110.141,-86.063,True,0.05,0.481,0.133,0,0.481,0.133,0,23,48.0,200.68,-1.53,-1.56,720,3630,2063
|
||||
123.303755452,1785272177.7033165,120.0,robust,tracking,True,1.22,2.82,1.815294335,1.7860767164,19.1,42.8,38.8,6,5,2.02,683871.81,683869.856,111.056,-85.022,True,0.05,1.151,0.085,0,1.151,0.085,0,23,64.0,169.36,-1.535,-1.56,722,3640,2063
|
||||
125.304219262,1785272179.703781,122.0,robust,tracking,True,1.22,2.82,1.8152578155,1.785570688,11.11,42.7,38.6,6,5,-8.82,683688.289,683696.831,110.802,-85.693,True,0.05,0.637,0.051,0,0.637,0.051,0,23,8.0,158.16,-1.537,-1.56,724,3650,2063
|
||||
127.304583545,1785272181.704145,124.0,robust,tracking,True,1.22,2.82,1.8152215994,1.7850628719,11.13,42.7,38.8,6,5,-1.48,683523.082,683524.519,110.235,-86.917,True,0.05,0.775,0.068,0,0.775,0.068,0,23,40.0,151.87,-1.538,-1.56,726,3660,2063
|
||||
129.508905566,1785272183.9084663,126.0,robust,tracking,True,1.22,2.82,1.8151838123,1.7845085023,12.03,43.1,38.6,6,5,9.01,683342.021,683333.296,110.201,-87.019,True,0.05,0.331,0.094,0,0.331,0.094,0,23,0.0,157.45,-1.537,-1.56,728,3670,2063
|
||||
131.500762179,1785272185.9003232,128.0,robust,tracking,True,1.22,2.82,1.8151487619,1.7840006032,12.83,42.6,38.6,6,5,3.63,683160.888,683157.371,110.357,-86.937,True,0.05,0.389,0.075,0,0.389,0.075,0,23,32.0,171.39,-1.535,-1.56,730,3680,2063
|
||||
133.504344505,1785272187.9039054,130.0,robust,tracking,True,1.22,2.82,1.8151127862,1.7834955606,12.64,42.7,38.5,6,5,3.69,682986.797,682983.217,110.053,-87.63,True,0.05,0.791,0.01,0,0.791,0.01,0,23,0.0,175.65,-1.534,-1.56,732,3690,2063
|
||||
135.509832366,1785272189.9094036,132.0,robust,tracking,True,1.22,2.82,1.8150762158,1.7829890753,11.13,42.7,38.4,6,5,5.1,682813.848,682808.909,110.531,-87.346,True,0.05,0.079,0.034,0,0.079,0.034,0,23,16.0,162.81,-1.536,-1.56,734,3700,2063
|
||||
137.506720578,1785272191.9062817,134.0,robust,tracking,True,1.22,2.82,1.8150397064,1.7824837171,10.99,42.8,39.5,6,5,1.42,682635.318,682633.946,110.584,-87.635,True,0.05,0.038,0.097,0,0.038,0.097,0,23,32.0,155.09,-1.537,-1.56,736,3710,2063
|
||||
139.504410571,1785272193.9039717,136.0,robust,tracking,True,1.22,2.82,1.8150041072,1.7819780719,11.15,42.7,38.1,6,5,-6.72,682453.853,682460.364,110.886,-87.452,True,0.05,0.412,0.407,0,0.412,0.407,0,23,48.0,139.96,-1.539,-1.56,738,3720,2063
|
||||
141.503078598,1785272195.9026399,138.0,robust,tracking,True,1.22,2.82,1.8149699154,1.781475266,11.11,42.7,38.8,6,5,11.45,682296.086,682284.991,109.638,-89.46,True,0.05,1.419,0.005,0,1.419,0.005,0,23,40.0,167.17,-1.535,-1.56,740,3730,2063
|
||||
143.500493916,1785272197.900055,140.0,robust,tracking,True,1.22,2.82,1.8149354252,1.7809734693,11.02,42.9,38.7,6,5,12.89,682120.284,682107.799,110.911,-87.736,True,0.05,0.88,0.067,0,0.88,0.067,0,23,56.0,162.67,-1.536,-1.56,742,3740,2063
|
||||
145.512927625,1785272199.9124885,142.0,robust,tracking,True,1.22,2.82,1.8149005029,1.7804693538,10.96,42.9,39.4,6,5,-25.89,681906.117,681931.202,110.881,-87.866,True,0.05,0.775,0.078,0,0.775,0.078,0,23,8.0,107.83,-1.544,-1.56,744,3750,2063
|
||||
147.506278297,1785272201.9058392,144.0,robust,tracking,True,1.22,2.82,1.8148661739,1.7799667491,10.98,43.0,39.0,6,5,-5.66,681746.195,681751.678,109.788,-89.613,True,0.05,1.009,0.035,0,1.009,0.035,0,23,56.0,152.27,-1.538,-1.56,746,3760,2063
|
||||
149.505513865,1785272203.9050748,146.0,robust,tracking,True,1.22,2.82,1.8148312963,1.779461219,11.25,42.4,38.4,6,5,-7.95,681563.852,681571.554,110.854,-89.368,True,0.05,0.482,0.099,0,0.482,0.099,0,23,56.0,142.08,-1.539,-1.56,748,3770,2063
|
||||
151.504103133,1785272205.903664,148.0,robust,tracking,True,1.22,2.82,1.8147976667,1.778959756,10.98,43.0,39.4,6,5,11.06,681403.38,681392.662,110.425,-89.358,True,0.05,0.199,0.058,0,0.199,0.058,0,23,16.0,161.29,-1.536,-1.56,750,3780,2063
|
||||
153.502847324,1785272207.9024084,150.0,robust,tracking,True,1.22,2.82,1.8147608671,1.7784571849,15.23,42.6,38.9,6,5,9.66,681223.637,681214.276,110.279,-89.721,True,0.05,0.371,0.218,0,0.371,0.218,0,23,48.0,172.12,-1.535,-1.56,752,3790,2063
|
||||
155.506297511,1785272209.9058583,152.0,robust,tracking,True,1.22,2.82,1.8147266277,1.7779565566,11.07,42.6,38.7,6,5,0.35,681034.417,681034.073,110.263,-89.772,True,0.05,0.371,0.13,0,0.371,0.13,0,23,56.0,180.04,-1.533,-1.56,754,3800,2063
|
||||
157.500753602,1785272211.9003143,154.0,robust,tracking,True,1.22,2.82,1.814692541,1.777454151,11.13,42.4,38.4,6,5,8.94,680862.299,680853.639,110.737,-89.433,True,0.05,0.755,0.053,0,0.755,0.053,0,23,0.0,180.5,-1.533,-1.56,756,3810,2063
|
||||
159.504450759,1785272213.904012,156.0,robust,tracking,True,1.22,2.82,1.8146585955,1.7769539906,11.22,42.3,38.2,6,5,14.47,680691.31,680677.294,110.442,-90.141,True,0.05,0.405,0.02,0,0.405,0.02,0,23,32.0,196.03,-1.531,-1.56,758,3820,2063
|
||||
161.510423884,1785272215.9099848,158.0,robust,tracking,True,1.21,2.82,1.8146253251,1.7764523761,14.07,42.3,38.6,6,5,14.17,680511.698,680497.972,110.887,-89.977,True,0.05,0.551,0.182,0,0.551,0.182,0,23,32.0,184.51,-1.532,-1.56,760,3830,2063
|
||||
163.504960681,1785272217.9045217,160.0,robust,tracking,True,1.21,2.81,1.8145907816,1.7759535814,10.99,42.6,39.7,6,5,9.57,680326.822,680317.554,110.339,-91.079,True,0.05,0.465,0.01,0,0.465,0.01,0,23,8.0,150.36,-1.538,-1.56,762,3840,2063
|
||||
165.507220868,1785272219.9067822,162.0,robust,tracking,True,1.21,2.81,1.8145551226,1.7754518476,10.98,42.8,38.9,6,5,5.13,680143.185,680138.216,109.975,-91.641,True,0.05,0.834,0.034,0,0.834,0.034,0,23,0.0,144.58,-1.539,-1.56,764,3850,2063
|
||||
167.503525545,1785272221.9030867,164.0,robust,tracking,True,1.21,2.81,1.8145228906,1.7749543943,14.36,42.8,38.7,6,5,4.72,679959.278,679954.706,110.179,-91.65,True,0.05,0.706,0.085,0,0.706,0.085,0,23,48.0,124.01,-1.542,-1.56,766,3860,2063
|
||||
169.50061802,1785272223.900179,166.0,robust,tracking,True,1.21,2.81,1.8144898435,1.7744531221,11.09,42.8,37.7,6,5,13.57,679787.48,679774.331,110.775,-91.606,True,0.05,1.106,0.075,0,1.106,0.075,0,23,56.0,145.18,-1.538,-1.56,768,3870,2063
|
||||
171.510388267,1785272225.9099493,168.0,robust,tracking,True,1.21,2.81,1.8144559568,1.7739550407,11.08,42.6,38.2,6,5,8.28,679599.18,679591.156,110.344,-91.505,True,0.05,0.285,0.062,0,0.285,0.062,0,23,56.0,137.18,-1.54,-1.56,770,3880,2063
|
||||
173.500668495,1785272227.9002295,170.0,robust,tracking,True,1.21,2.81,1.8144213808,1.7734561125,11.07,42.5,38.0,6,5,-7.05,679401.0,679407.83,110.355,-91.896,True,0.05,0.239,0.023,0,0.239,0.023,0,23,48.0,151.22,-1.538,-1.56,772,3890,2063
|
||||
175.509072834,1785272229.9086337,172.0,robust,tracking,True,1.21,2.81,1.8143875329,1.7729571416,14.88,42.6,38.0,6,5,-9.75,679214.759,679224.203,110.051,-92.203,True,0.05,0.388,0.024,0,0.388,0.024,0,23,16.0,127.04,-1.541,-1.56,774,3900,2063
|
||||
177.511375431,1785272231.9109368,174.0,robust,tracking,True,1.21,2.81,1.8143541178,1.7724598606,11.01,42.7,39.1,6,5,-28.57,679010.25,679037.93,110.131,-92.548,True,0.05,0.679,0.066,0,0.679,0.066,0,23,16.0,144.09,-1.539,-1.56,776,3910,2063
|
||||
179.5031023,1785272233.9026635,176.0,robust,tracking,True,1.21,2.81,1.8143216536,1.7719632746,11.04,42.6,38.1,6,5,-1.94,678850.304,678852.181,110.443,-92.232,True,0.05,0.439,0.164,0,0.439,0.164,0,23,24.0,156.61,-1.537,-1.56,778,3920,2063
|
||||
181.500919559,1785272235.9004805,178.0,robust,tracking,True,1.21,2.81,1.8142885244,1.7714647145,11.67,42.7,39.6,6,5,14.37,678680.998,678667.079,110.223,-93.101,True,0.05,0.927,0.048,0,0.927,0.048,0,23,24.0,174.87,-1.534,-1.56,780,3930,2063
|
||||
183.504154069,1785272237.903716,180.0,robust,tracking,True,1.21,2.81,1.8142515963,1.7709676955,11.12,42.4,38.5,6,5,3.52,678484.103,678480.688,110.013,-93.55,True,0.05,0.69,0.056,0,0.69,0.056,0,23,0.0,167.43,-1.535,-1.56,782,3940,2063
|
||||
185.509701079,1785272239.9092622,182.0,robust,tracking,True,1.21,2.81,1.8142225906,1.7704719824,11.12,42.5,38.1,6,5,2.8,678298.275,678295.56,110.36,-92.989,True,0.05,0.188,0.156,0,0.188,0.156,0,23,0.0,175.14,-1.534,-1.56,784,3950,2063
|
||||
187.511311613,1785272241.9108725,184.0,robust,tracking,True,1.21,2.81,1.8141898144,1.7699731912,11.11,42.3,38.5,6,5,-16.24,678094.632,678110.363,110.845,-93.036,True,0.05,0.46,0.009,0,0.46,0.009,0,23,32.0,150.91,-1.537,-1.56,786,3960,2063
|
||||
189.504094586,1785272243.9036555,186.0,robust,tracking,True,1.21,2.81,1.8141560815,1.7694787336,11.13,42.5,37.9,6,5,-1.22,677924.284,677925.468,110.012,-93.838,True,0.05,0.542,0.031,0,0.542,0.031,0,23,32.0,167.75,-1.535,-1.56,788,3970,2063
|
||||
191.503264307,1785272245.902825,188.0,robust,tracking,True,1.21,2.81,1.8141223587,1.7689845528,11.12,42.4,38.3,6,5,12.44,677751.751,677739.703,110.673,-93.502,True,0.05,0.691,0.123,0,0.691,0.123,0,23,32.0,181.14,-1.533,-1.56,790,3980,2063
|
||||
193.500850983,1785272247.9004118,190.0,robust,tracking,True,1.21,2.81,1.8140919959,1.768488668,14.03,42.3,38.2,6,5,8.52,677559.555,677551.298,110.495,-93.954,True,0.05,0.376,0.004,0,0.376,0.004,0,23,56.0,155.05,-1.537,-1.56,792,3990,2063
|
||||
195.507436225,1785272249.9069972,192.0,robust,tracking,True,1.21,2.81,1.8140572992,1.7679932608,10.97,42.9,38.9,6,5,-25.61,677336.289,677361.101,110.053,-94.87,True,0.05,0.565,0.132,0,0.565,0.132,0,23,0.0,143.1,-1.539,-1.56,794,4000,2063
|
||||
197.710664279,1785272252.1102252,194.0,robust,tracking,True,1.21,2.81,1.8140224199,1.7674512561,11.06,42.7,38.3,6,5,-21.21,677131.244,677151.796,110.436,-94.668,True,0.05,0.467,0.099,0,0.467,0.099,0,23,16.0,127.66,-1.541,-1.56,797,4010,2063
|
||||
199.904494084,1785272254.3040552,196.0,robust,tracking,True,1.21,2.81,1.8139878446,1.7669062479,11.21,42.4,37.9,6,5,2.5,676945.184,676942.759,110.719,-94.592,True,0.05,0.329,0.254,0,0.329,0.254,0,23,0.0,166.66,-1.535,-1.56,799,4020,2063
|
||||
201.902748265,1785272256.3023093,198.0,robust,tracking,True,1.21,2.8,1.8139551469,1.7664122598,19.71,42.7,38.2,6,5,3.44,676756.483,676753.152,110.284,-95.508,True,0.05,0.834,0.075,0,0.834,0.075,0,23,48.0,159.1,-1.536,-1.56,801,4030,2063
|
||||
203.900860006,1785272258.3004212,200.0,robust,tracking,True,1.21,2.8,1.8139243484,1.7659203155,21.27,42.6,37.9,6,5,16.7,676577.254,676561.076,111.005,-94.638,True,0.05,0.65,0.026,0,0.65,0.026,0,23,40.0,185.48,-1.532,-1.56,803,4040,2063
|
||||
205.904311168,1785272260.303872,202.0,robust,tracking,True,1.21,2.8,1.8138926701,1.7654272361,11.23,42.3,37.5,6,5,-4.45,676364.571,676368.878,111.007,-94.853,True,0.05,0.681,0.154,0,0.681,0.154,0,23,32.0,146.48,-1.538,-1.56,805,4050,2063
|
||||
207.911054786,1785272262.3106158,204.0,robust,tracking,True,1.21,2.8,1.8138607249,1.7649342967,11.29,42.2,37.1,6,5,-0.42,676178.038,676178.445,111.268,-95.121,True,0.05,0.669,0.187,0,0.669,0.187,0,23,56.0,148.37,-1.538,-1.56,807,4060,2063
|
||||
209.904852911,1785272264.304414,206.0,robust,tracking,True,1.21,2.8,1.8138277745,1.7644418155,17.32,42.6,38.4,6,5,5.29,675993.222,675988.099,110.799,-95.578,True,0.05,0.772,0.012,0,0.772,0.012,0,23,48.0,167.53,-1.535,-1.56,809,4070,2063
|
||||
211.904389957,1785272266.3039508,208.0,robust,tracking,True,1.2,2.8,1.8137949136,1.7639490637,14.88,42.5,38.1,6,5,4.76,675802.554,675797.942,109.97,-97.009,True,0.05,1.186,0.157,0,1.186,0.157,0,23,16.0,161.37,-1.536,-1.56,811,4080,2063
|
||||
213.908887407,1785272268.3084483,210.0,robust,tracking,True,1.2,2.8,1.8137652527,1.7634585771,13.65,41.9,38.5,6,5,-19.97,675583.7,675603.041,110.734,-96.484,True,0.05,0.11,0.069,0,0.11,0.069,0,23,8.0,134.36,-1.54,-1.56,813,4090,2063
|
||||
215.900878981,1785272270.3004403,212.0,robust,tracking,True,1.2,2.8,1.8137346862,1.7629700707,13.14,42.4,39.0,6,5,1.39,675411.049,675409.707,110.477,-96.41,True,0.05,0.489,0.149,0,0.489,0.149,0,23,48.0,164.12,-1.535,-1.56,815,4100,2063
|
||||
217.908715503,1785272272.3082767,214.0,robust,tracking,True,1.2,2.8,1.8137007679,1.7624770049,11.19,42.1,38.1,6,5,-3.51,675214.305,675217.703,110.227,-97.513,True,0.05,0.667,0.022,0,0.667,0.022,0,23,56.0,141.12,-1.539,-1.56,817,4110,2063
|
||||
219.904589627,1785272274.3041506,216.0,robust,tracking,True,1.2,2.8,1.8136705307,1.7619846326,17.3,42.2,38.8,6,5,-14.02,675007.687,675021.273,110.312,-97.218,True,0.05,0.452,0.069,0,0.452,0.069,0,23,24.0,158.28,-1.536,-1.56,819,4120,2063
|
||||
221.9039367,1785272276.3034978,218.0,robust,tracking,True,1.2,2.8,1.8136409999,1.7614933749,11.12,42.1,39.2,6,5,-3.83,674821.953,674825.66,111.06,-96.371,True,0.05,1.326,0.028,0,1.326,0.028,0,23,48.0,170.5,-1.534,-1.56,821,4130,2063
|
||||
223.91008895,1785272278.3096502,220.0,robust,tracking,True,1.2,2.8,1.8136105372,1.7610043295,11.73,42.3,38.7,6,5,0.13,674630.412,674630.288,111.079,-97.301,True,0.05,0.408,0.148,0,0.408,0.148,0,23,56.0,157.28,-1.536,-1.56,823,4140,2063
|
||||
225.902916967,1785272280.302478,222.0,robust,tracking,True,1.2,2.8,1.8135794516,1.7605142092,21.26,41.9,39.1,6,5,-21.67,674413.179,674434.176,111.065,-97.127,True,0.05,0.752,0.068,0,0.752,0.068,0,23,48.0,124.73,-1.541,-1.56,825,4150,2063
|
||||
227.901755726,1785272282.3013165,224.0,robust,tracking,True,1.2,2.8,1.8135490001,1.7600248185,11.02,42.5,39.3,6,5,13.8,674252.975,674239.611,110.715,-97.719,True,0.05,0.748,0.078,0,0.748,0.078,0,23,32.0,159.45,-1.536,-1.56,827,4160,2063
|
||||
229.901619429,1785272284.3011806,226.0,robust,tracking,True,1.2,2.8,1.8135177823,1.7595391681,11.05,42.4,39.1,6,5,12.06,674053.956,674042.275,110.355,-98.939,True,0.05,0.78,0.043,0,0.78,0.043,0,23,56.0,170.95,-1.534,-1.56,829,4170,2063
|
||||
231.90481137,1785272286.3043725,228.0,robust,tracking,True,1.2,2.8,1.8134877896,1.7590492494,11.09,42.4,38.1,6,5,12.72,673856.473,673844.146,110.721,-98.388,True,0.05,0.538,0.188,0,0.538,0.188,0,23,48.0,170.54,-1.534,-1.56,831,4180,2063
|
||||
233.905326912,1785272288.3048878,230.0,robust,tracking,True,1.2,2.8,1.8134570418,1.7585604429,11.06,42.5,38.9,6,5,5.44,673653.112,673647.84,110.098,-98.963,True,0.05,0.301,0.311,0,0.301,0.311,0,23,8.0,171.59,-1.534,-1.56,833,4190,2063
|
||||
235.904458614,1785272290.3040195,232.0,robust,tracking,True,1.2,2.8,1.8134261204,1.7580735849,11.06,42.5,38.5,6,5,-4.62,673445.637,673450.117,110.459,-98.844,True,0.05,0.829,0.028,0,0.829,0.028,0,23,0.0,153.73,-1.537,-1.56,835,4200,2063
|
||||
237.904071483,1785272292.3036325,234.0,robust,tracking,True,1.2,2.8,1.8133959704,1.7575868747,12.8,42.0,37.7,6,5,-7.87,673243.852,673251.478,110.772,-98.845,True,0.05,0.559,0.037,0,0.559,0.037,0,23,0.0,146.96,-1.538,-1.56,837,4210,2063
|
||||
239.908760739,1785272294.3083217,236.0,robust,tracking,True,1.2,2.8,1.8133666091,1.7570976374,11.1,42.3,38.6,6,5,4.99,673058.134,673053.301,110.565,-99.434,True,0.05,0.506,0.016,0,0.506,0.016,0,23,24.0,168.65,-1.535,-1.56,839,4220,2063
|
||||
241.900563008,1785272296.300124,238.0,robust,tracking,True,1.2,2.79,1.8133344617,1.7566107514,11.14,42.3,38.4,6,5,-14.57,672839.788,672853.907,110.365,-99.522,True,0.05,0.253,0.141,0,0.253,0.141,0,23,0.0,147.65,-1.538,-1.56,841,4230,2063
|
||||
243.904645899,1785272298.3042068,240.0,robust,tracking,True,1.2,2.79,1.8133057846,1.7561227378,11.1,42.2,38.8,6,5,3.18,672658.12,672655.039,109.903,-100.274,True,0.05,0.476,0.033,0,0.476,0.033,0,23,32.0,164.64,-1.535,-1.56,843,4240,2063
|
||||
245.904621193,1785272300.3041825,242.0,robust,tracking,True,1.2,2.79,1.8132777969,1.7556370904,11.14,42.2,38.7,6,5,-0.81,672454.836,672455.618,110.573,-100.187,True,0.05,0.405,0.013,0,0.405,0.013,0,23,16.0,134.41,-1.54,-1.56,845,4250,2063
|
||||
247.899436713,1785272302.2989979,244.0,robust,tracking,True,1.2,2.79,1.8132463791,1.7551512789,11.16,42.2,39.0,6,5,13.48,672269.138,672256.082,110.183,-100.179,True,0.05,0.565,0.113,0,0.565,0.113,0,23,40.0,169.04,-1.535,-1.56,847,4260,2063
|
||||
249.911369626,1785272304.3109307,246.0,robust,tracking,True,1.2,2.79,1.813217727,1.754665179,11.11,42.5,39.2,6,5,-6.09,672049.638,672055.536,109.774,-100.683,True,0.05,0.486,0.021,0,0.486,0.021,0,23,0.0,135.93,-1.539,-1.56,849,4270,2063
|
||||
251.903384606,1785272306.3029459,248.0,robust,tracking,True,1.2,2.79,1.8131881994,1.7541830169,11.18,42.2,39.0,6,5,10.96,671865.57,671854.952,109.778,-101.09,True,0.05,0.278,0.027,0,0.278,0.027,0,23,48.0,182.36,-1.532,-1.56,851,4280,2063
|
||||
253.900781619,1785272308.3003423,250.0,robust,tracking,True,1.2,2.79,1.8131589097,1.7536951988,11.17,42.3,38.9,6,5,3.55,671659.189,671655.749,110.155,-100.561,True,0.05,0.584,0.008,0,0.584,0.008,0,23,16.0,141.85,-1.539,-1.56,853,4290,2063
|
||||
255.910281973,1785272310.3098433,252.0,robust,tracking,True,1.2,2.79,1.8131293194,1.753212297,11.2,42.1,38.4,6,5,7.57,671462.581,671455.25,109.95,-101.447,True,0.05,0.189,0.096,0,0.189,0.096,0,23,16.0,141.76,-1.539,-1.56,855,4300,2063
|
||||
257.904754461,1785272312.3043153,254.0,robust,tracking,True,1.2,2.79,1.8131029159,1.7527265372,11.15,42.4,38.7,6,5,-6.95,671245.456,671252.194,110.05,-101.621,True,0.05,0.518,0.084,0,0.518,0.084,0,23,56.0,134.53,-1.54,-1.56,857,4310,2063
|
||||
259.904898749,1785272314.3044596,256.0,robust,tracking,True,1.2,2.79,1.8130720433,1.7522445406,11.3,41.8,38.4,6,5,-4.74,671044.584,671049.173,110.162,-101.624,True,0.05,0.281,0.225,0,0.281,0.225,0,23,0.0,142.99,-1.539,-1.56,859,4320,2063
|
||||
261.904207465,1785272316.3037684,258.0,robust,tracking,True,1.19,2.79,1.8130440645,1.7517582107,11.31,42.0,38.4,6,5,-7.29,670837.352,670844.416,110.701,-101.27,True,0.05,0.844,0.152,0,0.844,0.152,0,23,56.0,167.54,-1.535,-1.56,861,4330,2063
|
||||
263.903658409,1785272318.3032198,260.0,robust,tracking,True,1.19,2.79,1.8130134772,1.7512773631,11.34,42.0,38.5,6,5,-1.16,670637.855,670638.976,110.001,-102.901,True,0.05,0.697,0.02,0,0.697,0.02,0,23,48.0,166.85,-1.535,-1.56,863,4340,2063
|
||||
265.906678701,1785272320.3062396,262.0,robust,tracking,True,1.19,2.79,1.8129886444,1.7507938348,14.74,42.5,38.9,6,5,10.12,670442.306,670432.505,110.568,-101.927,True,0.05,0.908,0.068,0,0.908,0.068,0,23,40.0,192.58,-1.531,-1.56,865,4350,2063
|
||||
267.908740259,1785272322.308301,264.0,robust,tracking,True,1.19,2.79,1.8129614176,1.750311896,11.27,42.3,37.7,6,5,-11.12,670213.856,670224.632,110.96,-102.072,True,0.05,0.648,0.323,0,0.648,0.323,0,23,56.0,163.77,-1.535,-1.56,867,4360,2063
|
||||
269.904666955,1785272324.304228,266.0,robust,tracking,True,1.19,2.79,1.8129315339,1.7498285268,11.35,41.9,37.9,6,5,0.23,670016.459,670016.233,110.584,-102.496,True,0.05,0.457,0.073,0,0.457,0.073,0,23,48.0,200.12,-1.53,-1.56,869,4370,2063
|
||||
271.904417064,1785272326.303978,268.0,robust,tracking,True,1.19,2.79,1.8129033974,1.7493462127,11.29,42.0,38.2,6,5,-11.63,669798.53,669809.801,110.923,-102.202,True,0.05,0.955,0.178,0,0.955,0.178,0,23,16.0,154.9,-1.537,-1.56,871,4380,2063
|
||||
273.904287144,1785272328.3038483,270.0,robust,tracking,True,1.19,2.79,1.8128759146,1.7488645046,11.24,42.2,38.4,6,5,0.32,669604.755,669604.44,110.23,-103.356,True,0.05,0.559,0.043,0,0.559,0.043,0,23,56.0,147.74,-1.538,-1.56,873,4390,2063
|
||||
275.905752343,1785272330.3053133,272.0,robust,tracking,True,1.19,2.79,1.8128467845,1.7483871309,11.29,42.1,38.0,6,5,0.52,669397.133,669396.627,110.767,-103.125,True,0.05,0.839,0.033,0,0.839,0.033,0,23,48.0,175.42,-1.533,-1.56,875,4400,2063
|
||||
277.901721957,1785272332.3012831,274.0,robust,tracking,True,1.19,2.79,1.8128179501,1.7479069779,11.23,42.2,38.8,6,5,-15.46,669172.771,669187.748,110.557,-103.518,True,0.05,0.505,0.152,0,0.505,0.152,0,23,8.0,141.55,-1.539,-1.56,877,4410,2063
|
||||
279.901270286,1785272334.3008313,276.0,robust,tracking,True,1.19,2.79,1.8127918148,1.7474239577,11.26,42.2,38.9,6,5,5.21,668986.416,668981.367,110.705,-103.667,True,0.05,0.415,0.13,0,0.415,0.13,0,23,56.0,164.7,-1.535,-1.56,879,4420,2063
|
||||
281.911805396,1785272336.3113663,278.0,robust,tracking,True,1.19,2.78,1.812764459,1.7469430861,11.14,42.3,38.6,6,5,9.2,668783.357,668774.443,110.765,-103.653,True,0.05,0.976,0.047,0,0.976,0.047,0,23,40.0,167.02,-1.535,-1.56,881,4430,2063
|
||||
283.904627638,1785272338.3041887,280.0,robust,tracking,True,1.19,2.78,1.8127364977,1.7464643722,11.13,42.4,38.9,6,5,1.9,668567.589,668565.747,110.702,-104.554,True,0.05,0.496,0.084,0,0.496,0.084,0,23,56.0,150.73,-1.537,-1.56,883,4440,2063
|
||||
285.904540282,1785272340.3041015,282.0,robust,tracking,True,1.19,2.78,1.8127111868,1.7459831701,11.26,42.2,38.2,6,5,-21.52,668336.38,668357.228,110.484,-104.5,True,0.05,0.4,0.023,0,0.4,0.023,0,23,16.0,127.92,-1.541,-1.56,885,4450,2063
|
||||
287.903906607,1785272342.3034678,284.0,robust,tracking,True,1.19,2.78,1.8126838601,1.7455066962,11.09,42.3,39.2,6,5,-0.35,668145.403,668145.742,110.548,-104.863,True,0.05,0.13,0.213,0,0.13,0.213,0,23,0.0,156.5,-1.536,-1.56,887,4460,2063
|
||||
289.903519609,1785272344.3030806,286.0,robust,tracking,True,1.19,2.78,1.8126545436,1.74502695,11.28,42.2,38.0,6,5,-17.14,667916.867,667933.474,110.923,-105.07,True,0.05,0.412,0.112,0,0.412,0.112,0,23,8.0,145.17,-1.538,-1.56,889,4470,2063
|
||||
291.906994829,1785272346.3065557,288.0,robust,tracking,True,1.19,2.78,1.812627295,1.7445482935,12.84,42.5,38.6,6,5,-1.31,667721.551,667722.816,109.629,-106.134,True,0.05,0.721,0.164,0,0.721,0.164,0,23,40.0,158.89,-1.536,-1.56,891,4480,2063
|
||||
293.904251457,1785272348.3038123,290.0,robust,tracking,True,1.19,2.78,1.8126009474,1.7440682755,11.16,42.3,38.2,6,5,14.39,667524.719,667510.777,110.381,-105.665,True,0.05,0.161,0.079,0,0.161,0.079,0,23,32.0,184.17,-1.532,-1.56,893,4490,2063
|
||||
295.904732818,1785272350.3042939,292.0,robust,tracking,True,1.19,2.78,1.8125748376,1.7435915248,11.06,42.6,38.1,6,5,-9.72,667289.655,667299.068,110.248,-105.873,True,0.05,0.291,0.005,0,0.291,0.005,0,23,32.0,159.99,-1.536,-1.56,895,4500,2063
|
||||
297.910797167,1785272352.3103583,294.0,robust,tracking,True,1.19,2.78,1.8125472744,1.7431147283,11.0,42.9,38.4,6,5,9.37,667097.136,667088.06,110.084,-106.181,True,0.05,0.237,0.074,0,0.237,0.074,0,23,16.0,168.11,-1.535,-1.56,897,4510,2063
|
||||
299.905108443,1785272354.3046699,296.0,robust,tracking,True,1.19,2.78,1.812521259,1.7426375476,16.12,42.5,37.8,6,5,-12.01,666863.837,666875.472,110.096,-106.613,True,0.05,0.693,0.156,0,0.693,0.156,0,23,64.0,124.56,-1.541,-1.56,899,4520,2063
|
||||
|
+52
@@ -0,0 +1,52 @@
|
||||
# Genesys2 live PPS telemetry replay
|
||||
|
||||
Source: `pps_end_to_end_validation/hardware_logs/genesys2_pps_20260728.csv`
|
||||
|
||||
## Direct firmware/RTL observations
|
||||
|
||||
- Samples/duration: 149 / 296.0 s.
|
||||
- Status interval: 2.000 s; median sequence step 10 (2.0 s of 200 ms solves).
|
||||
- Profiles: robust; locked fraction 1.000; transitions 0; reference SVIDs [23].
|
||||
- TDOP/GDOP medians: 1.220 / 2.820; clock sigma median 11.24 m.
|
||||
- C/N0 mean/min medians: 42.6 / 38.4 dB-Hz; tracked/used medians 6/5.
|
||||
- WLS innovation RMS/p95-absolute: 10.47 / 19.66 m.
|
||||
- Raw/filtered bias slopes: -90.020 / -90.025 m/s; PPS velocity residual median 0.079 m/s.
|
||||
- Internal hardware phase mean: 155.37 ns; median-centered RMS/p95/max/p-p: 17.92 / 31.26 / 47.95 / 93.21 ns.
|
||||
- Ten-second averaged phase wander p-p: 46.16 ns.
|
||||
- Sampled phase-step RMS/p95/max: 22.84 / 44.13 / 56.38 ns. These are at the status interval, not consecutive physical 1 PPS edges.
|
||||
- Rate command median/span: -1.5370 / 0.0140 ppm; integrator median -1.5600 ppm.
|
||||
- Fixed-point PI rate reproduction RMS/p95/max: 0.430 / 0.852 / 0.921 ppb. This is the difference between logged `rate_cmd` and `integrator + Kp*hw_phase`; the remaining error is dominated by three-decimal UART rounding.
|
||||
- Phase correlation with rate command / WLS innovation: 0.988 / 0.656.
|
||||
- A constant oscillator inferred from zero net frequency is 1.536779 ppm. Integrating the logged command under that assumption gives centered phase RMS/p-p 23.65 / 92.48 ns.
|
||||
|
||||
`hw_phase` is the RTL PI residual captured over APB. It is not a trusted-generator time-interval measurement of the output pin, so these values cannot certify physical PPS jitter or absolute offset.
|
||||
|
||||
## Telemetry-conditioned end-to-end loop replay
|
||||
|
||||
The replay imports the first locked phase/rate/integrator and software filter snapshot instead of incorrectly resetting the board to an acquisition state. It uses raw/filtered clock bias, Doppler datum drift, TDOP/GDOP, clock sigma, C/N0, satellite count, FDE, reference SVID and the production fixed-point coefficients. The reference trajectory is reconstructed from filtered bias, the logged PI residual and the integrated rate command.
|
||||
|
||||
- Expanded updates: 1481; interpolation factor 9.94.
|
||||
- Snapshot starts locked at 0.00 s; post-warmup locked fraction 1.000; unlocks 0.
|
||||
- Constant-oscillator convention: 1.536779 ppm.
|
||||
- Modeled physical-grid edge centered RMS/p95/p-p: 41.32 / 80.00 / 168.00 ns.
|
||||
- Modeled one-second period RMS/p95/max: 5.51 / 8.00 / 16.00 ns.
|
||||
- Modeled PI residual centered RMS/p95/p-p: 21.47 / 46.79 / 135.63 ns.
|
||||
- Logged-epoch PI residual reproduction RMS/p95 error: 18.07 / 44.30 ns.
|
||||
- Logged-epoch filtered-bias reproduction RMS/p95: 9.87 / 22.13 m.
|
||||
|
||||
## Tuning sensitivity on the same trace
|
||||
|
||||
| tuning | edge RMS (ns) | edge p-p (ns) | period max (ns) | residual reproduction RMS (ns) | unlocks |
|
||||
|---|---:|---:|---:|---:|---:|
|
||||
| current | 41.32 | 168.00 | 16.00 | 18.07 | 0 |
|
||||
| robust_bias_1_64 | 57.06 | 216.00 | 16.00 | 24.53 | 0 |
|
||||
| drift_gain_1_2 | 42.23 | 168.00 | 16.00 | 18.03 | 0 |
|
||||
|
||||
These counterfactuals reuse a reference trajectory reconstructed from the current filter, and nine of every ten solve epochs are interpolated. They are useful as a rejection test, not as evidence to deploy new gains. On this capture neither `robust_bias_1_64` nor `drift_gain_1_2` improves the current loop's absolute wander.
|
||||
|
||||
## Limits and next measurement
|
||||
|
||||
- Missing status epochs are linearly interpolated when sequence step is greater than one. This preserves low-frequency wander but understates unlogged 200 ms noise; it is not an exact firmware replay.
|
||||
- The inferred oscillator and active phase assume one constant crystal frequency chosen to make net phase slope zero over this capture. Temperature telemetry was unavailable, so slow crystal motion is not separable from the reconstructed reference.
|
||||
- Level-1 telemetry does not expose ABS_SAMPLE, REM_CODE_PHASE, full capture counter, chi-square ratio, pseudorange residual vector, board temperature, or a trusted PPS edge timestamp. The bit/subframe part of the end-to-end model remains synthetic for those fields.
|
||||
- Set `GPS_PPS_STATUS_LOG_INTERVAL=1` for exact 200 ms input cadence. For a max-20 ns pin claim, capture DUT and trusted PPS with a common-clock TIC/scope after cable/output-delay calibration.
|
||||
+1482
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,25 @@
|
||||
scenario,tuning,runs,lock_p95_s,absolute_rms_median_ns,absolute_max_p95_ns,absolute_peak_to_peak_median_ns,period_rms_median_ns,period_p95_median_ns,period_max_worst_ns,wander_10s_peak_to_peak_median_ns,profile_transitions_total,unlocks_total
|
||||
real_geometry_board_noise,current,24,10.770000000000001,10.881176406988356,32.0,48.00000000000001,3.867084378182096,8.0,8.000000000000002,36.8,0,0
|
||||
real_geometry_board_noise,robust_noise_guard,24,10.770000000000001,10.881176406988356,32.0,48.00000000000001,3.867084378182096,8.0,8.000000000000002,36.8,0,0
|
||||
real_geometry_board_noise,quiet_precision,24,10.770000000000001,6.311779001549132,16.000000000000004,24.0,2.7197398634103065,7.999999999999999,8.000000000000005,20.0,0,0
|
||||
real_geometry_board_noise,fast_precision,24,10.770000000000001,11.482151620012546,40.0,56.0,4.4110037618184545,8.0,16.0,40.0,0,0
|
||||
real_geometry_board_noise,drift_gain_1_2,24,10.770000000000001,9.830551108590587,32.0,40.0,3.761748764716249,8.0,8.000000000000002,33.6,0,0
|
||||
real_geometry_board_noise,quality_scheduled_drift,24,10.770000000000001,9.830551108590587,32.0,40.0,3.761748764716249,8.0,8.000000000000002,33.6,0,0
|
||||
scope_like_multipath,current,24,10.770000000000001,22.943340809908015,62.79999999999998,88.0,4.727734084565672,8.000000000000002,16.0,80.80000000000001,0,0
|
||||
scope_like_multipath,robust_noise_guard,24,10.770000000000001,22.943340809908015,62.79999999999998,88.0,4.727734084565672,8.000000000000002,16.0,80.80000000000001,0,0
|
||||
scope_like_multipath,quiet_precision,24,10.770000000000001,17.147532219212493,40.0,63.99999999999999,3.7617487647162484,8.000000000000004,8.000000000000005,58.4,0,0
|
||||
scope_like_multipath,fast_precision,24,10.770000000000001,22.40714058089752,56.00000000000001,92.0,5.040539175116031,8.000000000000002,16.0,78.0,0,0
|
||||
scope_like_multipath,drift_gain_1_2,24,10.8,21.354092092763203,62.79999999999998,88.0,4.465388951979694,8.0,16.0,74.80000000000001,0,0
|
||||
scope_like_multipath,quality_scheduled_drift,24,10.8,21.354092092763203,62.79999999999998,88.0,4.465388951979694,8.0,16.0,74.80000000000001,0,0
|
||||
quality_transitions,current,24,10.770000000000001,18.351896116449183,96.0,80.0,4.4110037618184545,8.0,23.99999999999999,73.20000000000002,72,0
|
||||
quality_transitions,robust_noise_guard,24,10.770000000000001,17.481868755279223,80.0,80.0,4.374396022586886,8.0,16.00000000000001,67.6,72,0
|
||||
quality_transitions,quiet_precision,24,10.770000000000001,10.467771592924619,56.0,47.99999999999999,3.0800577533580347,7.999999999999999,16.0,42.0,72,0
|
||||
quality_transitions,fast_precision,24,10.770000000000001,19.145424296464835,88.00000000000001,88.0,4.861884148700504,8.0,24.000000000000004,73.6,72,0
|
||||
quality_transitions,drift_gain_1_2,24,10.770000000000001,16.291766736261422,83.39999999999999,76.0,4.089452052592408,8.0,16.000000000000004,63.599999999999994,72,0
|
||||
quality_transitions,quality_scheduled_drift,24,10.770000000000001,16.766578539927025,83.39999999999999,80.0,4.224793965697071,8.0,16.00000000000001,67.6,72,0
|
||||
thermal_0p05ppm,current,24,10.770000000000001,39.2873108676108,102.79999999999998,136.0,5.727479384554999,8.000000000000005,24.0,124.0,0,0
|
||||
thermal_0p05ppm,robust_noise_guard,24,10.770000000000001,39.2873108676108,102.79999999999998,136.0,5.727479384554999,8.000000000000005,24.0,124.0,0,0
|
||||
thermal_0p05ppm,quiet_precision,24,10.770000000000001,210.25812707207118,416.0,648.0,13.000191801484194,24.000000000000004,32.0,632.4,0,0
|
||||
thermal_0p05ppm,fast_precision,24,10.770000000000001,21.850277755888733,63.39999999999999,96.0,5.82476895848467,8.000000000000002,16.000000000000004,77.60000000000001,0,0
|
||||
thermal_0p05ppm,drift_gain_1_2,24,10.770000000000001,38.40828815100511,96.0,136.0,5.656836393570529,8.000000000000005,16.000000000000004,122.0,0,0
|
||||
thermal_0p05ppm,quality_scheduled_drift,24,10.770000000000001,38.40828815100511,96.0,136.0,5.656836393570529,8.000000000000005,16.000000000000004,122.0,0,0
|
||||
|
@@ -0,0 +1,70 @@
|
||||
# PPS stability analysis: current RTL + 180 ms firmware model
|
||||
|
||||
This report is generated by `stability_analysis.py`. The absolute LNAV epoch path and the stochastic control loop are evaluated separately so a 20 ms semantic error cannot be hidden by loop noise.
|
||||
|
||||
## Absolute epoch result
|
||||
|
||||
- Current RTL delivered the validated subframe after 9 following bits.
|
||||
- With 180 ms postponement, reference error was -0.000032 ms.
|
||||
- DUT minus trusted PPS was 32.01 ns. That remainder is the model's configured four-PCLK capture pipeline, not a measured board delay.
|
||||
- Production firmware and this analysis both define `POSTPONE_TOW` as 180 ms for the current RTL delivery semantics.
|
||||
|
||||
## Hardware data used by this synthetic sweep
|
||||
|
||||
- Real GPS capture: 989 GDOP samples and 990 tracked-satellite samples over about five minutes.
|
||||
- GDOP min/median/p95/max: 1.86 / 1.93 / 3.15 / 3.15.
|
||||
- Tracked satellites: 8..8 (median 8).
|
||||
- Horizontal scatter RMS/p95/max: 8.58 / 15.15 / 23.77 m. No surveyed truth was recorded.
|
||||
- The older fusion capture used by this sweep has no ABS_SAMPLE, REM_CODE_PHASE, TDOP/HDOP, C/N0, pseudorange residual, PPS phase/rate/integrator, temperature, or trusted-edge TIC samples. Those columns in the companion CSV are explicitly synthetic. Newer PPS status captures are analyzed separately by `hardware_replay.py` and are not silently mixed into this historical baseline.
|
||||
|
||||
Synthetic timing uses the board-calibration values already encoded in the PPS tests: TDOP 1.50, GDOP 2.92, clock sigma 15.87 m (52.94 ns), C/N0 40.3/34.8 dB-Hz, drift noise 3.47 m/s, seven-satellite reference point, -291.75 m/s datum range rate, and 1.592 ppm local oscillator. Real GDOP and satellite count replace the geometry fields when available.
|
||||
|
||||
## Current-loop results
|
||||
|
||||
`period max` is the worst |edge[n]-edge[n-1]| across all seeds. `absolute p-p` and `10 s wander p-p` expose the slow left/right motion that period RMS hides.
|
||||
|
||||
| scenario | abs RMS median (ns) | abs p-p median (ns) | 10 s wander p-p (ns) | period RMS (ns) | period max (ns) | unlocks |
|
||||
|---|---:|---:|---:|---:|---:|---:|
|
||||
| real_geometry_board_noise | 10.88 | 48.00 | 36.80 | 3.87 | 8.00 | 0 |
|
||||
| scope_like_multipath | 22.94 | 88.00 | 80.80 | 4.73 | 16.00 | 0 |
|
||||
| quality_transitions | 18.35 | 80.00 | 73.20 | 4.41 | 24.00 | 0 |
|
||||
| thermal_0p05ppm | 39.29 | 136.00 | 124.00 | 5.73 | 24.00 | 0 |
|
||||
|
||||
The loop does not numerically oscillate or lose lock in these runs. The scope-like symptom is low-frequency phase wander: it can be tens of nanoseconds while one-second period jitter remains below 20 ns. Therefore a `p95 period jitter < 20 ns` assertion is not sufficient for an absolute ±20 ns requirement.
|
||||
|
||||
## Linear stability
|
||||
|
||||
The fixed-point coefficients give the following discrete poles at the 200 ms navigation update rate:
|
||||
|
||||
| profile | natural frequency | damping | poles | max magnitude |
|
||||
|---|---:|---:|---|---:|
|
||||
| acquire | 0.120 Hz | 0.90 | 0.852913+0.033242j, 0.852913-0.033242j | 0.853561 |
|
||||
| precision | 0.035 Hz | 0.90 | 0.959449+0.017031j, 0.959449-0.017031j | 0.959600 |
|
||||
| robust | 0.012 Hz | 1.00 | 0.986668+0.000000j, 0.982945+0.000000j | 0.986668 |
|
||||
|
||||
All magnitudes are below one. Profile changes preserve the PI integrator, and the RTL snapshots Kp/Ki/max-slew/lock limits together with the next commit, so the current acquire/precision/robust switching is structurally sound. Stable individual modes do not prove arbitrary-switching stability, but the asymmetric 8-update upgrade / 3-update downgrade hysteresis and the stress runs show no coefficient chatter or unlock.
|
||||
|
||||
## Dynamic parameter sweep
|
||||
|
||||
| tuning | worst period max (ns) | median abs RMS over scenarios (ns) | transitions | unlocks |
|
||||
|---|---:|---:|---:|---:|
|
||||
| current | 24.00 | 20.65 | 72 | 0 |
|
||||
| robust_noise_guard | 24.00 | 20.21 | 72 | 0 |
|
||||
| quiet_precision | 32.00 | 13.81 | 72 | 0 |
|
||||
| fast_precision | 24.00 | 20.50 | 72 | 0 |
|
||||
| drift_gain_1_2 | 16.00 | 18.82 | 72 | 0 |
|
||||
| quality_scheduled_drift | 16.00 | 19.06 | 72 | 0 |
|
||||
|
||||
The safest small candidate is `robust_noise_guard`: retain the current 0.035 Hz precision loop and 0.012 Hz robust loop, but reduce only the robust-profile WLS residual gain from 1/32 to 1/64. In the synthetic quality-transition case its worst period step changed from 24.00 to 16.00 ns. This is a candidate for hardware A/B testing, not enough evidence for a production constant change.
|
||||
|
||||
The best synthetic all-scenario period result is `quality_scheduled_drift`: use drift gain 1/2 only with a precision-quality Doppler solution, fall back to the current 1/4 gain in robust quality, and combine that with the 1/64 robust bias gain. Its worst period step was 16.00 ns with no unlocks. It is the most meaningful dynamic A/B candidate, but the repository has no real drift-covariance or TIC samples with which to validate the threshold.
|
||||
|
||||
Narrowing precision to 0.012 Hz improves stationary noise but lags even the explicit 0.05 ppm/120 s synthetic oscillator variation. Widening it improves oscillator tracking but passes more WLS/multipath motion. A geometry-only bandwidth scheduler cannot resolve that tradeoff.
|
||||
|
||||
## Recommended architecture update
|
||||
|
||||
1. Keep current acquire/precision/robust PI profiles until real TIC and oscillator-temperature data are available. Do not tune from scope appearance alone.
|
||||
2. Log the existing firmware status line at every solve during validation: ABS_SAMPLE, REM_CODE_PHASE, capture sequence/counter, TDOP/HDOP/GDOP, C/N0, pseudorange residual RMS, raw/filtered bias, innovation, bias drift, hardware phase, rate, integrator, profile, lock, FPGA/oscillator temperature, DUT edge and trusted edge.
|
||||
3. A/B test the simulated scheduled filter first: precision drift gain 1/2, robust drift gain 1/4, precision bias gain 1/32, and robust bias gain 1/64. Longer term, use estimated phase/drift covariance to interpolate those software gains within bounds. Keep RTL PI bandwidth changes hysteretic and preserve its integrator as today.
|
||||
4. If temperature/frequency acceleration is the main source, add oscillator temperature feed-forward or a phase/frequency/frequency-drift estimator. Switching PI bandwidth alone either passes WLS wander or lags the oscillator.
|
||||
5. Define acceptance separately: calibrated mean offset, absolute centered error/p-p wander, one-second period error, holdover, and re-lock. A physical max-20 ns claim requires a common-clock TIC run after cable/output-delay calibration; this synthetic test cannot certify it.
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,874 @@
|
||||
"""Causal Linux model of the GPS LNAV-to-PPS timing path.
|
||||
|
||||
This module is intentionally narrower than a general GNSS simulator. It
|
||||
models the project boundary which can create a constant absolute-time error:
|
||||
|
||||
* parity-correct 30-bit GPS LNAV words, including D30* data inversion;
|
||||
* the 1 kHz prompt-sign transition detector and 20 ms bit-valid generator;
|
||||
* the tracking wrapper's 310-bit subframe/preamble search window;
|
||||
* current registered-window and proposed current-bit-aware validation;
|
||||
* subframe-ready and periodic navigation-solve interrupts;
|
||||
* RF ABS_SAMPLE anchoring and the timestamped 125 MHz PPS capture;
|
||||
* the firmware HOW + POSTPONE_TOW + STARTOFFSET - b/c mapping; and
|
||||
* coarse/normal PPS commits followed by comparison with an independent PPS.
|
||||
|
||||
Long stretches between 50 bit/s events are evaluated analytically. No result
|
||||
depends on the Linux scheduler or wall clock.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
from collections import deque
|
||||
from dataclasses import asdict, dataclass
|
||||
from enum import Enum
|
||||
from typing import Iterable, Sequence
|
||||
|
||||
|
||||
C = 299_792_458.0
|
||||
PREAMBLE = 0x8B
|
||||
LNAV_WORD_BITS = 30
|
||||
LNAV_DATA_BITS = 24
|
||||
LNAV_WORDS_PER_SUBFRAME = 10
|
||||
LNAV_SUBFRAME_BITS = LNAV_WORD_BITS * LNAV_WORDS_PER_SUBFRAME
|
||||
LNAV_BIT_PERIOD_S = 0.020
|
||||
LNAV_BITS_PER_SUBFRAME = 300
|
||||
LNAV_WINDOW_BITS = 2 + LNAV_SUBFRAME_BITS + 8
|
||||
TRACKING_INTEGRATION_PERIOD_S = 0.001
|
||||
TRACKING_INTEGRATIONS_PER_BIT = 20
|
||||
RF_HZ = 16_367_981.0
|
||||
PPS_HZ = 125_000_000.0
|
||||
START_OFFSET_S = 68.802e-3
|
||||
NAV_TOW_SNAP_TOLERANCE_S = 0.005
|
||||
Q32 = 1 << 32
|
||||
TICK_INTEGER_TOLERANCE = 0.01
|
||||
|
||||
|
||||
class DecoderMode(str, Enum):
|
||||
"""Subframe search ordering relative to the current bit-valid event."""
|
||||
|
||||
CURRENT_RTL = "current-rtl"
|
||||
CURRENT_BIT = "current-bit"
|
||||
|
||||
|
||||
def _bits(value: int, width: int) -> list[int]:
|
||||
return [(value >> shift) & 1 for shift in range(width - 1, -1, -1)]
|
||||
|
||||
|
||||
def _integer(bits: Iterable[int]) -> int:
|
||||
value = 0
|
||||
for bit in bits:
|
||||
value = (value << 1) | int(bit)
|
||||
return value
|
||||
|
||||
|
||||
def _xor(values: Iterable[int]) -> int:
|
||||
result = 0
|
||||
for value in values:
|
||||
result ^= int(value)
|
||||
return result
|
||||
|
||||
|
||||
def lnav_parity(data_bits: Sequence[int], previous_d29: int, previous_d30: int) -> tuple[int, ...]:
|
||||
"""Return transmitted D25*..D30* using the equations in tracking_wrapper.
|
||||
|
||||
``data_bits`` are normalized D1..D24. The equations below are a direct
|
||||
index-form transcription of ``parity_calc`` in tracking_wrapper.sv.
|
||||
"""
|
||||
|
||||
if len(data_bits) != LNAV_DATA_BITS:
|
||||
raise ValueError("an LNAV word must contain 24 data bits")
|
||||
d = [0] + [int(bit) for bit in data_bits] # ICD bit numbers are one based.
|
||||
return (
|
||||
_xor((previous_d29, d[1], d[2], d[3], d[5], d[6], d[10], d[11],
|
||||
d[12], d[13], d[14], d[17], d[18], d[20], d[23])),
|
||||
_xor((previous_d30, d[2], d[3], d[4], d[6], d[7], d[11], d[12],
|
||||
d[13], d[14], d[15], d[18], d[19], d[21], d[24])),
|
||||
_xor((previous_d29, d[1], d[3], d[4], d[5], d[7], d[8], d[12],
|
||||
d[13], d[14], d[15], d[16], d[19], d[20], d[22])),
|
||||
_xor((previous_d30, d[2], d[4], d[5], d[6], d[8], d[9], d[13],
|
||||
d[14], d[15], d[16], d[17], d[20], d[21], d[23])),
|
||||
_xor((previous_d30, d[1], d[3], d[5], d[6], d[7], d[9], d[10],
|
||||
d[14], d[15], d[16], d[17], d[18], d[21], d[22], d[24])),
|
||||
_xor((previous_d29, d[3], d[5], d[6], d[8], d[9], d[10], d[11],
|
||||
d[13], d[15], d[19], d[22], d[23], d[24])),
|
||||
)
|
||||
|
||||
|
||||
def encode_lnav_word(
|
||||
data: int, previous_d29: int, previous_d30: int
|
||||
) -> tuple[tuple[int, ...], int, int, int]:
|
||||
"""Encode one normalized 24-bit word and return transmitted bits/state."""
|
||||
|
||||
if data < 0 or data >= (1 << LNAV_DATA_BITS):
|
||||
raise ValueError("LNAV data does not fit in 24 bits")
|
||||
normalized = _bits(data, LNAV_DATA_BITS)
|
||||
parity = lnav_parity(normalized, previous_d29, previous_d30)
|
||||
transmitted_data = tuple(bit ^ previous_d30 for bit in normalized)
|
||||
transmitted = transmitted_data + parity
|
||||
normalized_word = (data << 6) | _integer(parity)
|
||||
return transmitted, parity[4], parity[5], normalized_word
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EncodedSubframe:
|
||||
z_count: int
|
||||
subframe_id: int
|
||||
transmitted_bits: tuple[int, ...]
|
||||
normalized_words: tuple[int, ...]
|
||||
ending_d29: int
|
||||
ending_d30: int
|
||||
|
||||
|
||||
def encode_lnav_subframe(
|
||||
z_count: int,
|
||||
subframe_id: int,
|
||||
previous_d29: int,
|
||||
previous_d30: int,
|
||||
*,
|
||||
week: int = 2200,
|
||||
) -> EncodedSubframe:
|
||||
"""Generate a deterministic, parity-valid LNAV subframe.
|
||||
|
||||
The TLM preamble, HOW Z-count/subframe ID, and the modulo-1024 week field
|
||||
used by this firmware are placed in their implemented bit positions. The
|
||||
remaining data is deterministic filler so all ten parity checks are live.
|
||||
"""
|
||||
|
||||
if not 0 <= z_count < (1 << 17):
|
||||
raise ValueError("HOW Z-count must fit in 17 bits")
|
||||
if not 1 <= subframe_id <= 5:
|
||||
raise ValueError("subframe ID must be 1..5")
|
||||
|
||||
data_words = [0] * LNAV_WORDS_PER_SUBFRAME
|
||||
data_words[0] = PREAMBLE << 16
|
||||
data_words[1] = (z_count << 7) | (subframe_id << 2)
|
||||
if subframe_id == 1:
|
||||
data_words[2] = (week & 0x3FF) << 14
|
||||
for index in range(2 if subframe_id != 1 else 3, LNAV_WORDS_PER_SUBFRAME):
|
||||
# Avoid changing the semantic fields above while exercising parity
|
||||
# with a stable, non-zero bit pattern.
|
||||
data_words[index] = (
|
||||
(0x5A_0000 ^ (z_count * 0x45D9F3B) ^ (subframe_id << 12) ^ index)
|
||||
& 0xFF_FFFF
|
||||
)
|
||||
|
||||
emitted: list[int] = []
|
||||
normalized_words: list[int] = []
|
||||
d29, d30 = int(previous_d29), int(previous_d30)
|
||||
for data in data_words:
|
||||
word, d29, d30, normalized_word = encode_lnav_word(data, d29, d30)
|
||||
emitted.extend(word)
|
||||
normalized_words.append(normalized_word)
|
||||
return EncodedSubframe(
|
||||
z_count=z_count,
|
||||
subframe_id=subframe_id,
|
||||
transmitted_bits=tuple(emitted),
|
||||
normalized_words=tuple(normalized_words),
|
||||
ending_d29=d29,
|
||||
ending_d30=d30,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DecodedSubframe:
|
||||
z_count: int
|
||||
subframe_id: int
|
||||
normalized_words: tuple[int, ...]
|
||||
|
||||
|
||||
def validate_rtl_window(window: Sequence[int]) -> DecodedSubframe | None:
|
||||
"""Mirror the RTL preamble and ten-word parity predicate.
|
||||
|
||||
The chronological window is ``previous D29*, D30*`` followed by the
|
||||
300-bit candidate subframe and eight bits of the following preamble.
|
||||
"""
|
||||
|
||||
if len(window) != LNAV_WINDOW_BITS:
|
||||
return None
|
||||
preamble = tuple(_bits(PREAMBLE, 8))
|
||||
inverse_preamble = tuple(bit ^ 1 for bit in preamble)
|
||||
first = tuple(window[2:10])
|
||||
following = tuple(window[302:310])
|
||||
if first not in (preamble, inverse_preamble):
|
||||
return None
|
||||
if following not in (preamble, inverse_preamble):
|
||||
return None
|
||||
|
||||
d29, d30 = int(window[0]), int(window[1])
|
||||
normalized_words: list[int] = []
|
||||
for word_index in range(LNAV_WORDS_PER_SUBFRAME):
|
||||
start = 2 + word_index * LNAV_WORD_BITS
|
||||
transmitted = tuple(int(bit) for bit in window[start : start + LNAV_WORD_BITS])
|
||||
normalized_data = tuple(bit ^ d30 for bit in transmitted[:LNAV_DATA_BITS])
|
||||
expected_parity = lnav_parity(normalized_data, d29, d30)
|
||||
if tuple(transmitted[LNAV_DATA_BITS:]) != expected_parity:
|
||||
return None
|
||||
normalized_words.append((_integer(normalized_data) << 6) | _integer(expected_parity))
|
||||
d29, d30 = transmitted[28], transmitted[29]
|
||||
|
||||
how_word = normalized_words[1]
|
||||
return DecodedSubframe(
|
||||
z_count=(how_word >> 13) & 0x1FFFF,
|
||||
subframe_id=(how_word >> 8) & 0x7,
|
||||
normalized_words=tuple(normalized_words),
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SampleStamp:
|
||||
raw: int
|
||||
corrected: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class BitEvent:
|
||||
stream_index: int
|
||||
value: int
|
||||
transmit_time_s: float
|
||||
receive_time_s: float
|
||||
sample_time_s: float
|
||||
sample: SampleStamp
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PromptSample:
|
||||
value: int
|
||||
receive_time_s: float
|
||||
sample: SampleStamp
|
||||
|
||||
|
||||
class BitGeneratorModel:
|
||||
"""Mirror bit_gen.sv at its 1 kHz valid-input clock enables."""
|
||||
|
||||
def __init__(self, *, propagation_s: float):
|
||||
self.propagation_s = propagation_s
|
||||
self.prompt_history: deque[int] = deque([0] * 40, maxlen=40)
|
||||
self.record_bits = False
|
||||
self.loop_counter = 0
|
||||
self.prompt_ones = 0
|
||||
self.absolute_sample: SampleStamp | None = None
|
||||
self.absolute_sample_time_s = 0.0
|
||||
self.output_index = 0
|
||||
|
||||
def feed(self, prompt: PromptSample) -> BitEvent | None:
|
||||
bit_transition = tuple(self.prompt_history) == (
|
||||
(1,) * TRACKING_INTEGRATIONS_PER_BIT
|
||||
+ (0,) * TRACKING_INTEGRATIONS_PER_BIT
|
||||
)
|
||||
loop_enabled = self.record_bits
|
||||
loop_full = self.loop_counter == TRACKING_INTEGRATIONS_PER_BIT - 1
|
||||
bit_value = int(self.prompt_ones > TRACKING_INTEGRATIONS_PER_BIT / 2 - 1)
|
||||
sampled_before_edge = self.absolute_sample
|
||||
sample_time_before_edge = self.absolute_sample_time_s
|
||||
|
||||
# All assignments in bit_gen are nonblocking: the output on a full
|
||||
# loop sees the ABS_SAMPLE registered by the preceding 1 ms input.
|
||||
self.prompt_history.append(prompt.value)
|
||||
self.absolute_sample = prompt.sample
|
||||
self.absolute_sample_time_s = prompt.receive_time_s
|
||||
if bit_transition:
|
||||
self.record_bits = True
|
||||
if loop_enabled:
|
||||
if loop_full:
|
||||
self.loop_counter = 0
|
||||
self.prompt_ones = 0
|
||||
else:
|
||||
self.loop_counter += 1
|
||||
self.prompt_ones += prompt.value
|
||||
|
||||
if not loop_full or sampled_before_edge is None:
|
||||
return None
|
||||
event = BitEvent(
|
||||
stream_index=self.output_index,
|
||||
value=bit_value,
|
||||
transmit_time_s=prompt.receive_time_s - self.propagation_s,
|
||||
receive_time_s=prompt.receive_time_s,
|
||||
sample_time_s=sample_time_before_edge,
|
||||
sample=sampled_before_edge,
|
||||
)
|
||||
self.output_index += 1
|
||||
return event
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SubframeInterrupt:
|
||||
z_count: int
|
||||
subframe_id: int
|
||||
words: tuple[int, ...]
|
||||
bit_index_after_how: int
|
||||
sample_time_s: float
|
||||
irq_time_s: float
|
||||
sample: SampleStamp
|
||||
|
||||
|
||||
class TrackingDecoderModel:
|
||||
"""Event-level model of tracking_wrapper's shift/search/IRQ ordering."""
|
||||
|
||||
def __init__(self, mode: DecoderMode, pps_hz: float = PPS_HZ):
|
||||
self.mode = DecoderMode(mode)
|
||||
self.pps_hz = pps_hz
|
||||
self.history: deque[int] = deque(maxlen=LNAV_WINDOW_BITS)
|
||||
|
||||
def feed(self, event: BitEvent) -> SubframeInterrupt | None:
|
||||
registered = tuple(self.history)
|
||||
if self.mode == DecoderMode.CURRENT_RTL:
|
||||
decoded = validate_rtl_window(registered)
|
||||
self.history.append(event.value)
|
||||
else:
|
||||
candidate = (registered + (event.value,))[-LNAV_WINDOW_BITS:]
|
||||
decoded = validate_rtl_window(candidate)
|
||||
self.history.append(event.value)
|
||||
if decoded is None:
|
||||
return None
|
||||
|
||||
how_boundary = decoded.z_count * 6.0
|
||||
bit_index = int(round((event.transmit_time_s - how_boundary) / LNAV_BIT_PERIOD_S))
|
||||
# load_sigs is edge-detected before gps_frame_ready. This two-PCLK
|
||||
# delay affects interrupt latency but not the latched ABS_SAMPLE.
|
||||
irq_time = event.receive_time_s + 2.0 / self.pps_hz
|
||||
return SubframeInterrupt(
|
||||
z_count=decoded.z_count,
|
||||
subframe_id=decoded.subframe_id,
|
||||
words=decoded.normalized_words,
|
||||
bit_index_after_how=bit_index,
|
||||
sample_time_s=event.sample_time_s,
|
||||
irq_time_s=irq_time,
|
||||
sample=event.sample,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CounterClock:
|
||||
nominal_hz: float
|
||||
epoch_time_s: float
|
||||
oscillator_ppm: float = 0.0
|
||||
|
||||
@property
|
||||
def physical_hz(self) -> float:
|
||||
return self.nominal_hz * (1.0 + self.oscillator_ppm * 1.0e-6)
|
||||
|
||||
def first_tick_at_or_after(self, time_s: float) -> int:
|
||||
exact = (time_s - self.epoch_time_s) * self.physical_hz
|
||||
nearest = round(exact)
|
||||
# Absolute GPS TOW is intentionally retained in the model. Subtracting
|
||||
# two ~345 ks binary64 values can leave a few thousandths of one 8 ns
|
||||
# tick even when the synthetic event was constructed exactly on that
|
||||
# tick. Snap only this numerical residue; genuinely asynchronous
|
||||
# events still use the following hardware clock edge.
|
||||
if abs(exact - nearest) <= TICK_INTEGER_TOLERANCE:
|
||||
exact = float(nearest)
|
||||
return max(0, int(math.ceil(exact)))
|
||||
|
||||
def time_for_tick(self, tick: int) -> float:
|
||||
return self.epoch_time_s + tick / self.physical_hz
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class NavSolveInterrupt:
|
||||
sample_time_s: float
|
||||
irq_time_s: float
|
||||
bit_index_after_how: int
|
||||
sample: SampleStamp
|
||||
pps_counter: int
|
||||
pps_capture_time_s: float
|
||||
pps_capture_sequence: int
|
||||
|
||||
|
||||
class NavSolveTimerModel:
|
||||
"""Mirror the current timer: seed from Z*300, then count later bit pulses."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
period_bits: int,
|
||||
subframe: SubframeInterrupt,
|
||||
clock: CounterClock,
|
||||
*,
|
||||
capture_pipeline_ticks: int,
|
||||
):
|
||||
if period_bits <= 0:
|
||||
raise ValueError("navigation solve period must be positive")
|
||||
self.period_bits = period_bits
|
||||
self.counter = (subframe.z_count * LNAV_BITS_PER_SUBFRAME) % period_bits
|
||||
self.start_sample_time_s = subframe.sample_time_s
|
||||
self.how_boundary_s = subframe.z_count * 6.0
|
||||
self.clock = clock
|
||||
self.capture_pipeline_ticks = capture_pipeline_ticks
|
||||
self.sequence = 0
|
||||
|
||||
def feed(self, event: BitEvent) -> NavSolveInterrupt | None:
|
||||
if event.sample_time_s <= self.start_sample_time_s:
|
||||
return None
|
||||
reached = self.counter == self.period_bits - 1
|
||||
self.counter = 0 if reached else self.counter + 1
|
||||
if not reached:
|
||||
return None
|
||||
|
||||
bit_tick = self.clock.first_tick_at_or_after(event.receive_time_s)
|
||||
capture_tick = bit_tick + self.capture_pipeline_ticks
|
||||
self.sequence += 1
|
||||
return NavSolveInterrupt(
|
||||
sample_time_s=event.sample_time_s,
|
||||
irq_time_s=self.clock.time_for_tick(capture_tick + 1),
|
||||
bit_index_after_how=int(
|
||||
round((event.transmit_time_s - self.how_boundary_s) / LNAV_BIT_PERIOD_S)
|
||||
),
|
||||
sample=event.sample,
|
||||
pps_counter=capture_tick,
|
||||
pps_capture_time_s=self.clock.time_for_tick(capture_tick),
|
||||
pps_capture_sequence=self.sequence,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FirmwareReference:
|
||||
nav_bit_index_after_how: int
|
||||
periodic_elapsed_s: float
|
||||
transmit_tow_s: float
|
||||
gps_time_at_capture_s: float
|
||||
gps_time_at_capture_q32_s: float
|
||||
true_capture_time_s: float
|
||||
signed_reference_error_s: float
|
||||
epoch_counter: int
|
||||
|
||||
|
||||
class FirmwareTimingModel:
|
||||
"""Mirror HOW sample anchoring and the current PPS receiver-time equation."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
nav_solve_period_bits: int,
|
||||
postpone_tow_s: float,
|
||||
propagation_s: float,
|
||||
):
|
||||
self.event_period_s = nav_solve_period_bits * LNAV_BIT_PERIOD_S
|
||||
self.postpone_tow_s = postpone_tow_s
|
||||
self.propagation_s = propagation_s
|
||||
# In this project's WLS datum STARTOFFSET - b/c recovers flight time.
|
||||
self.synthetic_clock_bias_m = (START_OFFSET_S - propagation_s) * C
|
||||
self.anchor: SubframeInterrupt | None = None
|
||||
self.rejected_nav_solves = 0
|
||||
|
||||
def on_subframe(self, event: SubframeInterrupt) -> None:
|
||||
self.anchor = event
|
||||
|
||||
@staticmethod
|
||||
def _signed_u32_delta(current: int, previous: int) -> int:
|
||||
delta = (current - previous) & 0xFFFF_FFFF
|
||||
return delta - (1 << 32) if delta & 0x8000_0000 else delta
|
||||
|
||||
def on_nav_solve(self, event: NavSolveInterrupt) -> FirmwareReference | None:
|
||||
if self.anchor is None:
|
||||
raise RuntimeError("navigation solve arrived before a valid HOW anchor")
|
||||
raw_delta = self._signed_u32_delta(event.sample.raw, self.anchor.sample.raw)
|
||||
anchor_fraction = self.anchor.sample.corrected - self.anchor.sample.raw
|
||||
event_fraction = event.sample.corrected - event.sample.raw
|
||||
elapsed_s = (raw_delta + event_fraction - anchor_fraction) / RF_HZ
|
||||
event_index = int(math.floor(elapsed_s / self.event_period_s + 0.5))
|
||||
periodic_elapsed_s = event_index * self.event_period_s
|
||||
if abs(periodic_elapsed_s - elapsed_s) > NAV_TOW_SNAP_TOLERANCE_S:
|
||||
# gps_time_from_periodic_sample_anchor() returns false and the
|
||||
# interrupt handler drops this otherwise coherent NAV solve.
|
||||
self.rejected_nav_solves += 1
|
||||
return None
|
||||
|
||||
transmit_tow = self.anchor.z_count * 6.0 + periodic_elapsed_s
|
||||
gps_reference = (
|
||||
transmit_tow
|
||||
+ self.postpone_tow_s
|
||||
+ START_OFFSET_S
|
||||
- self.synthetic_clock_bias_m / C
|
||||
)
|
||||
whole = math.floor(gps_reference)
|
||||
fraction_q32 = int(math.floor((gps_reference - whole) * Q32 + 0.5))
|
||||
if fraction_q32 == Q32:
|
||||
whole += 1
|
||||
fraction_q32 = 0
|
||||
gps_reference_q32 = whole + fraction_q32 / Q32
|
||||
return FirmwareReference(
|
||||
nav_bit_index_after_how=event.bit_index_after_how,
|
||||
periodic_elapsed_s=periodic_elapsed_s,
|
||||
transmit_tow_s=transmit_tow,
|
||||
gps_time_at_capture_s=gps_reference,
|
||||
gps_time_at_capture_q32_s=gps_reference_q32,
|
||||
true_capture_time_s=event.pps_capture_time_s,
|
||||
signed_reference_error_s=gps_reference_q32 - event.pps_capture_time_s,
|
||||
epoch_counter=event.pps_counter,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PpsEdge:
|
||||
gps_second: int
|
||||
dut_edge_time_s: float
|
||||
trusted_edge_time_s: float
|
||||
dut_minus_trusted_s: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PpsCoreResult:
|
||||
normal_commits_to_lock: int
|
||||
lock_time_s: float
|
||||
commit_age_s: float
|
||||
edges: tuple[PpsEdge, ...]
|
||||
|
||||
|
||||
def simulate_pps_core(
|
||||
references: Sequence[FirmwareReference],
|
||||
clock: CounterClock,
|
||||
*,
|
||||
compute_delay_s: float,
|
||||
core_pipeline_ticks: int,
|
||||
lock_updates: int,
|
||||
dut_output_delay_s: float,
|
||||
trusted_output_delay_s: float,
|
||||
) -> PpsCoreResult:
|
||||
"""Model timestamped coarse/normal commits and nominal locked PPS edges."""
|
||||
|
||||
if len(references) < lock_updates + 1:
|
||||
raise RuntimeError("not enough navigation references to acquire PPS lock")
|
||||
coarse = references[0]
|
||||
mapping_counter = coarse.epoch_counter
|
||||
mapping_time = coarse.gps_time_at_capture_q32_s
|
||||
|
||||
good_updates = 0
|
||||
lock_reference: FirmwareReference | None = None
|
||||
for reference in references[1:]:
|
||||
mapped_at_capture = mapping_time + (
|
||||
reference.epoch_counter - mapping_counter
|
||||
) / clock.nominal_hz
|
||||
phase_error = reference.gps_time_at_capture_q32_s - mapped_at_capture
|
||||
if abs(phase_error) <= 8.0 / clock.nominal_hz:
|
||||
good_updates += 1
|
||||
else:
|
||||
good_updates = 0
|
||||
if good_updates >= lock_updates:
|
||||
lock_reference = reference
|
||||
break
|
||||
if lock_reference is None:
|
||||
raise RuntimeError("PPS loop did not acquire lock")
|
||||
|
||||
commit_time = lock_reference.true_capture_time_s + compute_delay_s
|
||||
commit_tick = clock.first_tick_at_or_after(commit_time) + core_pipeline_ticks
|
||||
lock_time = clock.time_for_tick(commit_tick)
|
||||
commit_age_s = lock_time - lock_reference.true_capture_time_s
|
||||
lock_counter = commit_tick
|
||||
mapped_at_lock = mapping_time + (lock_counter - mapping_counter) / clock.nominal_hz
|
||||
|
||||
edges: list[PpsEdge] = []
|
||||
target_second = int(math.floor(mapped_at_lock)) + 1
|
||||
for _ in range(3):
|
||||
ticks_exact = (target_second - mapping_time) * clock.nominal_hz
|
||||
nearest_tick = round(ticks_exact)
|
||||
if abs(ticks_exact - nearest_tick) <= TICK_INTEGER_TOLERANCE:
|
||||
ticks_exact = float(nearest_tick)
|
||||
ticks_from_mapping = int(math.ceil(ticks_exact))
|
||||
edge_tick = mapping_counter + ticks_from_mapping
|
||||
dut_edge = clock.time_for_tick(edge_tick) + dut_output_delay_s
|
||||
trusted_edge = target_second + trusted_output_delay_s
|
||||
edges.append(
|
||||
PpsEdge(
|
||||
gps_second=target_second,
|
||||
dut_edge_time_s=dut_edge,
|
||||
trusted_edge_time_s=trusted_edge,
|
||||
dut_minus_trusted_s=dut_edge - trusted_edge,
|
||||
)
|
||||
)
|
||||
target_second += 1
|
||||
return PpsCoreResult(
|
||||
normal_commits_to_lock=good_updates,
|
||||
lock_time_s=lock_time,
|
||||
commit_age_s=commit_age_s,
|
||||
edges=tuple(edges),
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EndToEndConfig:
|
||||
decoder_mode: DecoderMode = DecoderMode.CURRENT_RTL
|
||||
postpone_tow_s: float = 0.180
|
||||
nav_solve_period_bits: int = 10
|
||||
propagation_s: float = 0.070
|
||||
pps_capture_pipeline_ticks: int = 4
|
||||
firmware_compute_delay_s: float = 0.010
|
||||
core_pipeline_ticks: int = 12
|
||||
dut_output_delay_s: float = 0.0
|
||||
trusted_output_delay_s: float = 0.0
|
||||
first_z_count: int = 57_600
|
||||
subframes: int = 3
|
||||
# Put the first HOW capture just before a modulo-32 wrap so the first
|
||||
# periodic solve also exercises the firmware's signed unwrap rule.
|
||||
rf_initial_sample: int = (
|
||||
(1 << 32) - int(10.25 * RF_HZ) - 1_000
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EndToEndResult:
|
||||
decoder_mode: str
|
||||
postpone_tow_ms: float
|
||||
bit_valid_events: int
|
||||
bit_generator_sample_lag_ms: float
|
||||
subframe_interrupts: int
|
||||
nav_solve_interrupts: int
|
||||
firmware_rejected_nav_solves: int
|
||||
first_subframe_readout_bits: int
|
||||
first_nav_solve_bit: int
|
||||
first_reference_error_ms: float
|
||||
expected_semantic_offset_ms: float
|
||||
pps: PpsCoreResult
|
||||
|
||||
|
||||
def _sample_at(time_s: float, epoch_s: float, initial_sample: int) -> SampleStamp:
|
||||
exact = initial_sample + (time_s - epoch_s) * RF_HZ
|
||||
raw_unwrapped = int(math.ceil(exact - 1.0e-10))
|
||||
fractional_delay = raw_unwrapped - exact
|
||||
raw = raw_unwrapped & 0xFFFF_FFFF
|
||||
# This is the firmware representation: corrected_abs is near the wrapped
|
||||
# integer ABS_SAMPLE, not a separately unwrapped floating-point counter.
|
||||
return SampleStamp(raw=raw, corrected=raw - fractional_delay)
|
||||
|
||||
|
||||
def _build_stream(config: EndToEndConfig) -> tuple[tuple[int, ...], float]:
|
||||
d29, d30 = 0, 0
|
||||
stream: list[int] = [d29, d30]
|
||||
for offset in range(config.subframes):
|
||||
subframe = encode_lnav_subframe(
|
||||
config.first_z_count + offset,
|
||||
offset % 5 + 1,
|
||||
d29,
|
||||
d30,
|
||||
)
|
||||
stream.extend(subframe.transmitted_bits)
|
||||
d29, d30 = subframe.ending_d29, subframe.ending_d30
|
||||
return tuple(stream), config.first_z_count * 6.0
|
||||
|
||||
|
||||
def _generate_bit_events(
|
||||
stream: Sequence[int],
|
||||
*,
|
||||
first_how_boundary_s: float,
|
||||
propagation_s: float,
|
||||
counter_epoch_s: float,
|
||||
rf_initial_sample: int,
|
||||
) -> tuple[BitEvent, ...]:
|
||||
"""Expand LNAV bits to the 1 kHz prompt-sign input consumed by bit_gen."""
|
||||
|
||||
first_bit_event_tx = first_how_boundary_s - (
|
||||
LNAV_SUBFRAME_BITS + 1
|
||||
) * LNAV_BIT_PERIOD_S
|
||||
first_prompt_receive = (
|
||||
first_bit_event_tx
|
||||
+ propagation_s
|
||||
- LNAV_BIT_PERIOD_S
|
||||
)
|
||||
prompt_values = (
|
||||
[1] * TRACKING_INTEGRATIONS_PER_BIT
|
||||
+ [0] * TRACKING_INTEGRATIONS_PER_BIT
|
||||
)
|
||||
for value in stream:
|
||||
prompt_values.extend([int(value)] * TRACKING_INTEGRATIONS_PER_BIT)
|
||||
# The first integration of the following bit makes loop_counter_full true
|
||||
# and emits the final requested bit, just as in bit_gen.sv.
|
||||
prompt_values.append(0)
|
||||
|
||||
training_duration_s = 2 * LNAV_BIT_PERIOD_S
|
||||
prompt_start_s = first_prompt_receive - training_duration_s
|
||||
generator = BitGeneratorModel(propagation_s=propagation_s)
|
||||
events: list[BitEvent] = []
|
||||
for prompt_index, value in enumerate(prompt_values):
|
||||
receive_time = (
|
||||
prompt_start_s
|
||||
+ prompt_index * TRACKING_INTEGRATION_PERIOD_S
|
||||
)
|
||||
output = generator.feed(
|
||||
PromptSample(
|
||||
value=value,
|
||||
receive_time_s=receive_time,
|
||||
sample=_sample_at(
|
||||
receive_time,
|
||||
counter_epoch_s,
|
||||
rf_initial_sample,
|
||||
),
|
||||
)
|
||||
)
|
||||
if output is not None:
|
||||
events.append(output)
|
||||
if tuple(event.value for event in events) != tuple(stream):
|
||||
raise RuntimeError("bit_gen model did not reproduce the requested LNAV stream")
|
||||
return tuple(events)
|
||||
|
||||
|
||||
def run_end_to_end(config: EndToEndConfig) -> EndToEndResult:
|
||||
if config.subframes < 2:
|
||||
raise ValueError("at least two subframes are required for preamble validation")
|
||||
stream, first_how_boundary = _build_stream(config)
|
||||
counter_epoch = first_how_boundary - 10.0
|
||||
# Oscillator/PI behavior is already covered by pps_loop/adaptive_pps. This
|
||||
# absolute-epoch regression uses an exact clock so a control-loop transient
|
||||
# cannot hide or imitate an integer navigation-bit error.
|
||||
clock = CounterClock(PPS_HZ, counter_epoch)
|
||||
bit_events = _generate_bit_events(
|
||||
stream,
|
||||
first_how_boundary_s=first_how_boundary,
|
||||
propagation_s=config.propagation_s,
|
||||
counter_epoch_s=counter_epoch,
|
||||
rf_initial_sample=config.rf_initial_sample,
|
||||
)
|
||||
decoder = TrackingDecoderModel(config.decoder_mode)
|
||||
firmware = FirmwareTimingModel(
|
||||
nav_solve_period_bits=config.nav_solve_period_bits,
|
||||
postpone_tow_s=config.postpone_tow_s,
|
||||
propagation_s=config.propagation_s,
|
||||
)
|
||||
|
||||
timer: NavSolveTimerModel | None = None
|
||||
subframe_events: list[SubframeInterrupt] = []
|
||||
nav_events: list[NavSolveInterrupt] = []
|
||||
references: list[FirmwareReference] = []
|
||||
for bit_event in bit_events:
|
||||
# An already-running timer sees the bit even when a later subframe is
|
||||
# validated on the same event. Firmware handles NAV_BITS before
|
||||
# NAV_SOLVE when both status flags share one interrupt.
|
||||
nav_event = timer.feed(bit_event) if timer is not None else None
|
||||
subframe_event = decoder.feed(bit_event)
|
||||
if subframe_event is not None:
|
||||
subframe_events.append(subframe_event)
|
||||
firmware.on_subframe(subframe_event)
|
||||
if timer is None:
|
||||
timer = NavSolveTimerModel(
|
||||
config.nav_solve_period_bits,
|
||||
subframe_event,
|
||||
clock,
|
||||
capture_pipeline_ticks=config.pps_capture_pipeline_ticks,
|
||||
)
|
||||
if nav_event is not None:
|
||||
nav_events.append(nav_event)
|
||||
reference = firmware.on_nav_solve(nav_event)
|
||||
if reference is not None:
|
||||
references.append(reference)
|
||||
|
||||
if not subframe_events or not nav_events or not references:
|
||||
raise RuntimeError("synthetic stream did not reach the PPS timing path")
|
||||
pps = simulate_pps_core(
|
||||
references,
|
||||
clock,
|
||||
compute_delay_s=config.firmware_compute_delay_s,
|
||||
core_pipeline_ticks=config.core_pipeline_ticks,
|
||||
lock_updates=5,
|
||||
dut_output_delay_s=config.dut_output_delay_s,
|
||||
trusted_output_delay_s=config.trusted_output_delay_s,
|
||||
)
|
||||
return EndToEndResult(
|
||||
decoder_mode=config.decoder_mode.value,
|
||||
postpone_tow_ms=config.postpone_tow_s * 1.0e3,
|
||||
bit_valid_events=len(bit_events),
|
||||
bit_generator_sample_lag_ms=(
|
||||
bit_events[0].receive_time_s - bit_events[0].sample_time_s
|
||||
)
|
||||
* 1.0e3,
|
||||
subframe_interrupts=len(subframe_events),
|
||||
nav_solve_interrupts=len(nav_events),
|
||||
firmware_rejected_nav_solves=firmware.rejected_nav_solves,
|
||||
first_subframe_readout_bits=subframe_events[0].bit_index_after_how,
|
||||
first_nav_solve_bit=nav_events[0].bit_index_after_how,
|
||||
first_reference_error_ms=references[0].signed_reference_error_s * 1.0e3,
|
||||
expected_semantic_offset_ms=(
|
||||
subframe_events[0].bit_index_after_how * LNAV_BIT_PERIOD_S
|
||||
- config.postpone_tow_s
|
||||
)
|
||||
* 1.0e3,
|
||||
pps=pps,
|
||||
)
|
||||
|
||||
|
||||
def default_comparison() -> tuple[EndToEndResult, ...]:
|
||||
return (
|
||||
run_end_to_end(EndToEndConfig()),
|
||||
run_end_to_end(
|
||||
EndToEndConfig(
|
||||
decoder_mode=DecoderMode.CURRENT_RTL,
|
||||
postpone_tow_s=0.160,
|
||||
)
|
||||
),
|
||||
run_end_to_end(
|
||||
EndToEndConfig(
|
||||
decoder_mode=DecoderMode.CURRENT_BIT,
|
||||
postpone_tow_s=0.160,
|
||||
)
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def _result_dict(result: EndToEndResult) -> dict[str, object]:
|
||||
value = asdict(result)
|
||||
value["pps"]["edges"] = [asdict(edge) for edge in result.pps.edges]
|
||||
return value
|
||||
|
||||
|
||||
def _print_table(results: Sequence[EndToEndResult]) -> None:
|
||||
print(
|
||||
"scenario bits correction reference error "
|
||||
"DUT - trusted PPS"
|
||||
)
|
||||
for result in results:
|
||||
offset_ms = result.pps.edges[0].dut_minus_trusted_s * 1.0e3
|
||||
scenario = f"{result.decoder_mode}/{result.postpone_tow_ms:.0f}ms"
|
||||
print(
|
||||
f"{scenario:<25} {result.first_subframe_readout_bits:>4d} "
|
||||
f"{result.postpone_tow_ms:>7.0f} ms "
|
||||
f"{result.first_reference_error_ms:>+12.6f} ms "
|
||||
f"{offset_ms:>+13.6f} ms"
|
||||
)
|
||||
print("\nPositive DUT - trusted means that the project PPS edge is late.")
|
||||
print(
|
||||
"The residual tens of nanoseconds are the configured PCLK capture "
|
||||
"pipeline, not an LNAV bit."
|
||||
)
|
||||
|
||||
|
||||
def main(argv: Sequence[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument(
|
||||
"--mode",
|
||||
choices=("matrix", DecoderMode.CURRENT_RTL.value, DecoderMode.CURRENT_BIT.value),
|
||||
default="matrix",
|
||||
help="run the three-way diagnosis matrix or one decoder mode",
|
||||
)
|
||||
parser.add_argument("--postpone-ms", type=float, default=180.0)
|
||||
parser.add_argument("--propagation-ms", type=float, default=70.0)
|
||||
parser.add_argument("--capture-pipeline-ticks", type=int, default=4)
|
||||
parser.add_argument("--compute-delay-ms", type=float, default=10.0)
|
||||
parser.add_argument("--dut-output-delay-ns", type=float, default=0.0)
|
||||
parser.add_argument("--trusted-output-delay-ns", type=float, default=0.0)
|
||||
parser.add_argument("--json", action="store_true")
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
if args.mode == "matrix":
|
||||
results = default_comparison()
|
||||
else:
|
||||
results = (
|
||||
run_end_to_end(
|
||||
EndToEndConfig(
|
||||
decoder_mode=DecoderMode(args.mode),
|
||||
postpone_tow_s=args.postpone_ms * 1.0e-3,
|
||||
propagation_s=args.propagation_ms * 1.0e-3,
|
||||
pps_capture_pipeline_ticks=args.capture_pipeline_ticks,
|
||||
firmware_compute_delay_s=args.compute_delay_ms * 1.0e-3,
|
||||
dut_output_delay_s=args.dut_output_delay_ns * 1.0e-9,
|
||||
trusted_output_delay_s=args.trusted_output_delay_ns * 1.0e-9,
|
||||
)
|
||||
),
|
||||
)
|
||||
if args.json:
|
||||
print(json.dumps([_result_dict(result) for result in results], indent=2))
|
||||
else:
|
||||
_print_table(results)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,907 @@
|
||||
#!/usr/bin/env python3
|
||||
"""End-to-end PPS epoch, telemetry, and closed-loop stability analysis.
|
||||
|
||||
The bit-level model proves the current-RTL/180 ms epoch convention. This
|
||||
module then drives the fixed-point firmware/RTL loop model with the telemetry
|
||||
that is present in the repository and deterministic synthetic replacements for
|
||||
the timing fields that were not captured on the board.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
from dataclasses import dataclass, replace
|
||||
import math
|
||||
from pathlib import Path
|
||||
import random
|
||||
import re
|
||||
import statistics
|
||||
from typing import Iterable
|
||||
|
||||
from pps_python.adaptive_pps import (
|
||||
ACQUIRE_PROFILE,
|
||||
PROFILES,
|
||||
FixedPointAdaptivePps,
|
||||
LoopProfile,
|
||||
Observation,
|
||||
Profile,
|
||||
QualityMetrics,
|
||||
TRACKING_HZ,
|
||||
classify_quality,
|
||||
)
|
||||
|
||||
from .pps_end_to_end import DecoderMode, EndToEndConfig, run_end_to_end
|
||||
|
||||
|
||||
RF_HZ = 16_367_981.0
|
||||
CODE_FREQ_HZ = 1_023_000.0
|
||||
SPEED_OF_LIGHT_MPS = 299_792_458.0
|
||||
UPDATE_PERIOD_S = 0.2
|
||||
NAV_BIT_PERIOD_S = 0.020
|
||||
BOARD_CLOCK_SIGMA_M = 15.87
|
||||
BOARD_TDOP = 1.50
|
||||
BOARD_GDOP = 2.92
|
||||
BOARD_CN0_MEAN_DBHZ = 40.3
|
||||
BOARD_CN0_MIN_DBHZ = 34.8
|
||||
BOARD_DRIFT_SIGMA_MPS = 3.47
|
||||
BOARD_OSCILLATOR_PPM = 1.592
|
||||
BOARD_REFERENCE_RANGE_RATE_MPS = -291.75
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RealTelemetry:
|
||||
gdop_time_s: tuple[float, ...]
|
||||
gdop: tuple[float, ...]
|
||||
tracked_time_s: tuple[float, ...]
|
||||
tracked: tuple[int, ...]
|
||||
horizontal_rms_m: float
|
||||
horizontal_p95_m: float
|
||||
horizontal_max_m: float
|
||||
|
||||
def gdop_at(self, time_s: float) -> float:
|
||||
return _linear_sample(self.gdop_time_s, self.gdop, time_s)
|
||||
|
||||
def tracked_at(self, time_s: float) -> int:
|
||||
if not self.tracked:
|
||||
return 8
|
||||
index = _previous_index(self.tracked_time_s, time_s)
|
||||
return self.tracked[index]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Scenario:
|
||||
name: str
|
||||
multipath: bool = False
|
||||
geometry_transitions: bool = False
|
||||
thermal_amplitude_ppm: float = 0.0
|
||||
thermal_period_s: float = 120.0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Tuning:
|
||||
name: str
|
||||
profiles: dict[Profile, LoopProfile]
|
||||
acquire: LoopProfile
|
||||
drift_gain: float
|
||||
robust_drift_gain: float | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SyntheticEpoch:
|
||||
scenario: str
|
||||
time_s: float
|
||||
abs_sample: int
|
||||
rem_code_phase: int
|
||||
corrected_abs_sample: float
|
||||
capture_counter_125: int
|
||||
capture_correction_ns: float
|
||||
used_satellites: int
|
||||
gdop: float
|
||||
hdop: float
|
||||
vdop: float
|
||||
tdop: float
|
||||
cn0_mean_dbhz: float
|
||||
cn0_min_dbhz: float
|
||||
pseudorange_noise_rms_m: float
|
||||
clock_bias_sigma_m: float
|
||||
clock_drift_error_mps: float
|
||||
timing_error_ns: float
|
||||
oscillator_ppm: float
|
||||
quality_profile: str
|
||||
observation: Observation
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RunMetrics:
|
||||
first_lock_s: float
|
||||
absolute_rms_ns: float
|
||||
absolute_p95_ns: float
|
||||
absolute_max_ns: float
|
||||
absolute_peak_to_peak_ns: float
|
||||
period_rms_ns: float
|
||||
period_p95_ns: float
|
||||
period_max_ns: float
|
||||
wander_10s_peak_to_peak_ns: float
|
||||
profile_transitions: int
|
||||
unlocks: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Aggregate:
|
||||
scenario: str
|
||||
tuning: str
|
||||
runs: int
|
||||
lock_p95_s: float
|
||||
absolute_rms_median_ns: float
|
||||
absolute_max_p95_ns: float
|
||||
absolute_peak_to_peak_median_ns: float
|
||||
period_rms_median_ns: float
|
||||
period_p95_median_ns: float
|
||||
period_max_worst_ns: float
|
||||
wander_10s_peak_to_peak_median_ns: float
|
||||
profile_transitions_total: int
|
||||
unlocks_total: int
|
||||
|
||||
|
||||
def _percentile(values: Iterable[float], fraction: float) -> float:
|
||||
ordered = sorted(values)
|
||||
if not ordered:
|
||||
return math.nan
|
||||
position = (len(ordered) - 1) * fraction
|
||||
lower = math.floor(position)
|
||||
upper = math.ceil(position)
|
||||
if lower == upper:
|
||||
return ordered[lower]
|
||||
weight = position - lower
|
||||
return ordered[lower] * (1.0 - weight) + ordered[upper] * weight
|
||||
|
||||
|
||||
def _previous_index(times: tuple[float, ...], time_s: float) -> int:
|
||||
if not times:
|
||||
return 0
|
||||
low = 0
|
||||
high = len(times)
|
||||
while low < high:
|
||||
middle = (low + high) // 2
|
||||
if times[middle] <= time_s:
|
||||
low = middle + 1
|
||||
else:
|
||||
high = middle
|
||||
return max(0, low - 1)
|
||||
|
||||
|
||||
def _linear_sample(
|
||||
times: tuple[float, ...], values: tuple[float, ...], time_s: float
|
||||
) -> float:
|
||||
if not values:
|
||||
return 1.93
|
||||
if time_s <= times[0]:
|
||||
return values[0]
|
||||
if time_s >= times[-1]:
|
||||
return values[-1]
|
||||
lower = _previous_index(times, time_s)
|
||||
upper = min(lower + 1, len(times) - 1)
|
||||
span = times[upper] - times[lower]
|
||||
if span <= 0.0:
|
||||
return values[lower]
|
||||
fraction = (time_s - times[lower]) / span
|
||||
return values[lower] + fraction * (values[upper] - values[lower])
|
||||
|
||||
|
||||
def load_real_telemetry(software_dir: Path) -> RealTelemetry:
|
||||
"""Load the only real GPS capture and its horizontal replay output."""
|
||||
log_path = software_dir / "fusion/logs/gps_live_5min_20260713.csv"
|
||||
comparison_path = (
|
||||
software_dir
|
||||
/ "fusion/output/model_compare_5min_20260713/model_comparison.csv"
|
||||
)
|
||||
gdop_time: list[float] = []
|
||||
gdop: list[float] = []
|
||||
tracked_time: list[float] = []
|
||||
tracked: list[int] = []
|
||||
if log_path.exists():
|
||||
with log_path.open(
|
||||
"r", encoding="utf-8", errors="replace", newline=""
|
||||
) as source:
|
||||
for row in csv.reader(source):
|
||||
if len(row) < 3:
|
||||
continue
|
||||
try:
|
||||
time_s = float(row[0])
|
||||
except ValueError:
|
||||
continue
|
||||
match = re.search(r"GDOP=([0-9.]+)", row[2])
|
||||
if match:
|
||||
gdop_time.append(time_s)
|
||||
gdop.append(float(match.group(1)))
|
||||
match = re.search(r"tracked satellites=(\d+)", row[2])
|
||||
if match:
|
||||
tracked_time.append(time_s)
|
||||
tracked.append(int(match.group(1)))
|
||||
|
||||
radii: list[float] = []
|
||||
if comparison_path.exists():
|
||||
with comparison_path.open("r", encoding="utf-8", newline="") as source:
|
||||
for row in csv.DictReader(source):
|
||||
try:
|
||||
radii.append(
|
||||
math.hypot(float(row["gps_e_m"]), float(row["gps_n_m"]))
|
||||
)
|
||||
except (KeyError, TypeError, ValueError):
|
||||
continue
|
||||
horizontal_rms = (
|
||||
math.sqrt(statistics.mean(value * value for value in radii))
|
||||
if radii
|
||||
else math.nan
|
||||
)
|
||||
return RealTelemetry(
|
||||
tuple(gdop_time),
|
||||
tuple(gdop),
|
||||
tuple(tracked_time),
|
||||
tuple(tracked),
|
||||
horizontal_rms,
|
||||
_percentile(radii, 0.95),
|
||||
max(radii, default=math.nan),
|
||||
)
|
||||
|
||||
|
||||
def scenario_matrix() -> tuple[Scenario, ...]:
|
||||
return (
|
||||
Scenario("real_geometry_board_noise"),
|
||||
Scenario("scope_like_multipath", multipath=True),
|
||||
Scenario("quality_transitions", geometry_transitions=True),
|
||||
Scenario("thermal_0p05ppm", thermal_amplitude_ppm=0.05),
|
||||
)
|
||||
|
||||
|
||||
def tuning_matrix() -> tuple[Tuning, ...]:
|
||||
current = dict(PROFILES)
|
||||
guarded = dict(PROFILES)
|
||||
guarded[Profile.ROBUST] = replace(
|
||||
guarded[Profile.ROBUST], software_bias_gain=1.0 / 64.0
|
||||
)
|
||||
quiet = dict(guarded)
|
||||
quiet[Profile.PRECISION] = replace(
|
||||
quiet[Profile.PRECISION],
|
||||
natural_frequency_hz=0.012,
|
||||
damping=1.0,
|
||||
software_bias_gain=1.0 / 16.0,
|
||||
)
|
||||
fast = dict(guarded)
|
||||
fast[Profile.PRECISION] = replace(
|
||||
fast[Profile.PRECISION], natural_frequency_hz=0.050, damping=1.0
|
||||
)
|
||||
return (
|
||||
Tuning("current", current, ACQUIRE_PROFILE, 1.0 / 4.0),
|
||||
Tuning("robust_noise_guard", guarded, ACQUIRE_PROFILE, 1.0 / 4.0),
|
||||
Tuning("quiet_precision", quiet, ACQUIRE_PROFILE, 1.0 / 4.0),
|
||||
Tuning("fast_precision", fast, ACQUIRE_PROFILE, 1.0 / 4.0),
|
||||
Tuning("drift_gain_1_2", guarded, ACQUIRE_PROFILE, 1.0 / 2.0),
|
||||
Tuning(
|
||||
"quality_scheduled_drift",
|
||||
guarded,
|
||||
ACQUIRE_PROFILE,
|
||||
1.0 / 2.0,
|
||||
1.0 / 4.0,
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def _weak_geometry(time_s: float) -> bool:
|
||||
return (80.0 <= time_s < 110.0) or (180.0 <= time_s < 205.0)
|
||||
|
||||
|
||||
def synthesize_epochs(
|
||||
telemetry: RealTelemetry,
|
||||
scenario: Scenario,
|
||||
*,
|
||||
duration_s: float,
|
||||
seed: int,
|
||||
update_period_s: float = UPDATE_PERIOD_S,
|
||||
) -> list[SyntheticEpoch]:
|
||||
"""Combine real GDOP/satellite count with documented board noise."""
|
||||
rng = random.Random(seed)
|
||||
result: list[SyntheticEpoch] = []
|
||||
count = round(duration_s / update_period_s)
|
||||
rf_initial = (1 << 32) - round(10.25 * RF_HZ) - 1000
|
||||
for update in range(count):
|
||||
time_s = update * update_period_s
|
||||
gdop = telemetry.gdop_at(time_s)
|
||||
satellites = max(4, min(8, telemetry.tracked_at(time_s)))
|
||||
tdop = gdop * (BOARD_TDOP / BOARD_GDOP)
|
||||
pdop = math.sqrt(max(0.0, gdop * gdop - tdop * tdop))
|
||||
vdop = 0.65 * pdop
|
||||
hdop = math.sqrt(max(0.0, pdop * pdop - vdop * vdop))
|
||||
cn0_mean = (
|
||||
BOARD_CN0_MEAN_DBHZ
|
||||
+ 1.2 * math.sin(2.0 * math.pi * time_s / 93.0)
|
||||
+ rng.gauss(0.0, 0.35)
|
||||
)
|
||||
cn0_min = cn0_mean - (
|
||||
BOARD_CN0_MEAN_DBHZ - BOARD_CN0_MIN_DBHZ
|
||||
)
|
||||
chi_square = 0.35
|
||||
velocity_residual = 0.23
|
||||
drift_sigma = BOARD_DRIFT_SIGMA_MPS
|
||||
if scenario.geometry_transitions and _weak_geometry(time_s):
|
||||
gdop = 8.0
|
||||
tdop = 5.0
|
||||
hdop = 4.3
|
||||
vdop = math.sqrt(max(0.0, gdop * gdop - tdop * tdop - hdop * hdop))
|
||||
satellites = 5
|
||||
cn0_mean = 27.0
|
||||
cn0_min = 18.0
|
||||
chi_square = 0.85
|
||||
velocity_residual = 1.8
|
||||
clock_sigma = 65.0
|
||||
drift_sigma = 5.0
|
||||
uere_sigma = clock_sigma / tdop
|
||||
else:
|
||||
uere_sigma = (
|
||||
(BOARD_CLOCK_SIGMA_M / BOARD_TDOP)
|
||||
* 10.0 ** ((BOARD_CN0_MEAN_DBHZ - cn0_mean) / 20.0)
|
||||
)
|
||||
clock_sigma = tdop * uere_sigma
|
||||
|
||||
pseudorange_errors = [
|
||||
rng.gauss(0.0, uere_sigma) for _ in range(satellites)
|
||||
]
|
||||
pseudorange_rms = math.sqrt(
|
||||
statistics.mean(value * value for value in pseudorange_errors)
|
||||
)
|
||||
long_omega = 2.0 * math.pi / 75.0
|
||||
short_omega = 2.0 * math.pi / 19.0
|
||||
true_datum_drift = (
|
||||
5.0 * math.sin(long_omega * time_s)
|
||||
+ 2.0 * math.sin(short_omega * time_s)
|
||||
)
|
||||
true_datum_offset = (
|
||||
5.0 * (1.0 - math.cos(long_omega * time_s)) / long_omega
|
||||
+ 2.0 * (1.0 - math.cos(short_omega * time_s)) / short_omega
|
||||
)
|
||||
timing_error_s = rng.gauss(0.0, clock_sigma / SPEED_OF_LIGHT_MPS)
|
||||
if scenario.multipath:
|
||||
timing_error_s += (
|
||||
25.0e-9 * math.sin(2.0 * math.pi * time_s / 67.0)
|
||||
+ 15.0e-9
|
||||
* math.sin(2.0 * math.pi * time_s / 29.0 + 0.8)
|
||||
)
|
||||
drift_error = true_datum_drift + rng.gauss(0.0, drift_sigma)
|
||||
oscillator_ppm = BOARD_OSCILLATOR_PPM + (
|
||||
scenario.thermal_amplitude_ppm
|
||||
* math.sin(2.0 * math.pi * time_s / scenario.thermal_period_s)
|
||||
)
|
||||
quality = QualityMetrics(
|
||||
tdop,
|
||||
gdop,
|
||||
clock_sigma,
|
||||
cn0_mean,
|
||||
cn0_min,
|
||||
chi_square,
|
||||
velocity_residual,
|
||||
satellites,
|
||||
)
|
||||
observation = Observation(
|
||||
time_s,
|
||||
timing_error_s,
|
||||
drift_error,
|
||||
quality,
|
||||
True,
|
||||
BOARD_REFERENCE_RANGE_RATE_MPS,
|
||||
1,
|
||||
true_datum_offset,
|
||||
)
|
||||
|
||||
continuous_abs = rf_initial + time_s * RF_HZ
|
||||
raw_unwrapped = math.ceil(continuous_abs - 1.0e-12)
|
||||
fractional_sample = raw_unwrapped - continuous_abs
|
||||
rem_code_phase = round(fractional_sample * CODE_FREQ_HZ)
|
||||
abs_sample = raw_unwrapped & 0xFFFF_FFFF
|
||||
corrected_abs = abs_sample - rem_code_phase / CODE_FREQ_HZ
|
||||
raw_capture_counter = math.floor(
|
||||
time_s * TRACKING_HZ * (1.0 + oscillator_ppm * 1.0e-6)
|
||||
)
|
||||
capture_correction_ticks = round(
|
||||
(rem_code_phase / CODE_FREQ_HZ) * (TRACKING_HZ / RF_HZ)
|
||||
)
|
||||
capture_counter = raw_capture_counter - capture_correction_ticks
|
||||
result.append(
|
||||
SyntheticEpoch(
|
||||
scenario.name,
|
||||
time_s,
|
||||
abs_sample,
|
||||
rem_code_phase,
|
||||
corrected_abs,
|
||||
capture_counter,
|
||||
capture_correction_ticks / TRACKING_HZ * 1.0e9,
|
||||
satellites,
|
||||
gdop,
|
||||
hdop,
|
||||
vdop,
|
||||
tdop,
|
||||
cn0_mean,
|
||||
cn0_min,
|
||||
pseudorange_rms,
|
||||
clock_sigma,
|
||||
drift_error,
|
||||
timing_error_s * 1.0e9,
|
||||
oscillator_ppm,
|
||||
classify_quality(quality).name.lower(),
|
||||
observation,
|
||||
)
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def _run_loop(
|
||||
epochs: list[SyntheticEpoch],
|
||||
tuning: Tuning,
|
||||
*,
|
||||
update_period_s: float = UPDATE_PERIOD_S,
|
||||
):
|
||||
model = FixedPointAdaptivePps(
|
||||
update_period_s=update_period_s,
|
||||
oscillator_ppm=BOARD_OSCILLATOR_PPM,
|
||||
profiles=tuning.profiles,
|
||||
acquire_profile=tuning.acquire,
|
||||
software_drift_gain=tuning.drift_gain,
|
||||
)
|
||||
for epoch in epochs:
|
||||
# Update at a solve boundary; the selected value applies to the
|
||||
# preceding synthetic interval, whose endpoint is this observation.
|
||||
model.oscillator_fraction = epoch.oscillator_ppm * 1.0e-6
|
||||
if tuning.robust_drift_gain is not None:
|
||||
target = classify_quality(epoch.observation.quality)
|
||||
model.software_drift_gain = (
|
||||
tuning.robust_drift_gain
|
||||
if target != Profile.PRECISION
|
||||
else tuning.drift_gain
|
||||
)
|
||||
model.update(epoch.observation)
|
||||
finish_s = epochs[-1].time_s + update_period_s if epochs else 0.0
|
||||
return model.finish(finish_s)
|
||||
|
||||
|
||||
def measure_trace(trace, *, after_s: float) -> RunMetrics:
|
||||
edge_errors = [
|
||||
error for _, error in trace.integer_second_edge_errors(after_s=after_s)
|
||||
]
|
||||
if len(edge_errors) < 12:
|
||||
raise RuntimeError("not enough locked integer-second PPS edges")
|
||||
center = statistics.median(edge_errors)
|
||||
centered = [value - center for value in edge_errors]
|
||||
periods = [
|
||||
edge_errors[index] - edge_errors[index - 1]
|
||||
for index in range(1, len(edge_errors))
|
||||
]
|
||||
moving_10s = [
|
||||
statistics.mean(centered[max(0, index - 9) : index + 1])
|
||||
for index in range(len(centered))
|
||||
]
|
||||
profiles = [
|
||||
profile
|
||||
for time_s, profile in zip(trace.time_s, trace.profile)
|
||||
if time_s >= after_s
|
||||
]
|
||||
profile_transitions = sum(
|
||||
profiles[index] != profiles[index - 1]
|
||||
for index in range(1, len(profiles))
|
||||
)
|
||||
return RunMetrics(
|
||||
trace.first_lock_time() if trace.first_lock_time() is not None else math.inf,
|
||||
math.sqrt(statistics.mean(value * value for value in centered)) * 1.0e9,
|
||||
_percentile((abs(value) for value in centered), 0.95) * 1.0e9,
|
||||
max(abs(value) for value in centered) * 1.0e9,
|
||||
(max(centered) - min(centered)) * 1.0e9,
|
||||
math.sqrt(statistics.mean(value * value for value in periods)) * 1.0e9,
|
||||
_percentile((abs(value) for value in periods), 0.95) * 1.0e9,
|
||||
max(abs(value) for value in periods) * 1.0e9,
|
||||
(max(moving_10s) - min(moving_10s)) * 1.0e9,
|
||||
profile_transitions,
|
||||
trace.unlock_events,
|
||||
)
|
||||
|
||||
|
||||
def aggregate(
|
||||
scenario: Scenario, tuning: Tuning, runs: list[RunMetrics]
|
||||
) -> Aggregate:
|
||||
return Aggregate(
|
||||
scenario.name,
|
||||
tuning.name,
|
||||
len(runs),
|
||||
_percentile((run.first_lock_s for run in runs), 0.95),
|
||||
statistics.median(run.absolute_rms_ns for run in runs),
|
||||
_percentile((run.absolute_max_ns for run in runs), 0.95),
|
||||
statistics.median(run.absolute_peak_to_peak_ns for run in runs),
|
||||
statistics.median(run.period_rms_ns for run in runs),
|
||||
statistics.median(run.period_p95_ns for run in runs),
|
||||
max(run.period_max_ns for run in runs),
|
||||
statistics.median(run.wander_10s_peak_to_peak_ns for run in runs),
|
||||
sum(run.profile_transitions for run in runs),
|
||||
sum(run.unlocks for run in runs),
|
||||
)
|
||||
|
||||
|
||||
def discrete_poles(
|
||||
profile: LoopProfile,
|
||||
update_period_s: float = UPDATE_PERIOD_S,
|
||||
) -> tuple[complex, complex]:
|
||||
"""Poles of the exact sample/commit ordering, before slew saturation."""
|
||||
kp_q46, ki_q46, _ = profile.fixed_coefficients(update_period_s)
|
||||
kp = kp_q46 / (1 << 46)
|
||||
ki = ki_q46 / (1 << 46)
|
||||
ticks = update_period_s * TRACKING_HZ
|
||||
a = 1.0 - ticks * (kp + ki)
|
||||
b = ticks
|
||||
c = -ki
|
||||
d = 1.0
|
||||
trace = a + d
|
||||
determinant = a * d - b * c
|
||||
discriminant = complex(trace * trace - 4.0 * determinant, 0.0) ** 0.5
|
||||
return (trace + discriminant) / 2.0, (trace - discriminant) / 2.0
|
||||
|
||||
|
||||
def write_telemetry(path: Path, epochs: list[SyntheticEpoch]) -> None:
|
||||
fields = [
|
||||
"scenario", "time_s", "abs_sample", "rem_code_phase",
|
||||
"corrected_abs_sample", "capture_counter_125", "used_satellites",
|
||||
"capture_correction_ns",
|
||||
"gdop", "hdop", "vdop", "tdop", "cn0_mean_dbhz",
|
||||
"cn0_min_dbhz", "pseudorange_noise_rms_m", "clock_bias_sigma_m",
|
||||
"clock_drift_error_mps", "timing_error_ns", "oscillator_ppm",
|
||||
"quality_profile",
|
||||
]
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", encoding="utf-8", newline="") as output:
|
||||
writer = csv.DictWriter(
|
||||
output, fieldnames=fields, lineterminator="\n"
|
||||
)
|
||||
writer.writeheader()
|
||||
for epoch in epochs:
|
||||
writer.writerow({field: getattr(epoch, field) for field in fields})
|
||||
|
||||
|
||||
def write_sweep(path: Path, results: list[Aggregate]) -> None:
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", encoding="utf-8", newline="") as output:
|
||||
writer = csv.DictWriter(
|
||||
output,
|
||||
fieldnames=list(Aggregate.__annotations__),
|
||||
lineterminator="\n",
|
||||
)
|
||||
writer.writeheader()
|
||||
for item in results:
|
||||
writer.writerow(item.__dict__)
|
||||
|
||||
|
||||
def _fmt(value: float) -> str:
|
||||
return "n/a" if not math.isfinite(value) else f"{value:.2f}"
|
||||
|
||||
|
||||
def write_report(
|
||||
path: Path,
|
||||
telemetry: RealTelemetry,
|
||||
results: list[Aggregate],
|
||||
epoch_result,
|
||||
update_period_s: float = UPDATE_PERIOD_S,
|
||||
) -> None:
|
||||
gdop = telemetry.gdop
|
||||
tracked = telemetry.tracked
|
||||
current = [item for item in results if item.tuning == "current"]
|
||||
guarded = [
|
||||
item for item in results if item.tuning == "robust_noise_guard"
|
||||
]
|
||||
lines = [
|
||||
"# PPS stability analysis: current RTL + 180 ms firmware model",
|
||||
"",
|
||||
"This report is generated by `stability_analysis.py`. The absolute "
|
||||
"LNAV epoch path and the stochastic control loop are evaluated "
|
||||
"separately so a 20 ms semantic error cannot be hidden by loop noise.",
|
||||
"",
|
||||
"## Absolute epoch result",
|
||||
"",
|
||||
f"- Current RTL delivered the validated subframe after "
|
||||
f"{epoch_result.first_subframe_readout_bits} following bits.",
|
||||
f"- With 180 ms postponement, reference error was "
|
||||
f"{epoch_result.first_reference_error_ms:.6f} ms.",
|
||||
f"- DUT minus trusted PPS was "
|
||||
f"{epoch_result.pps.edges[0].dut_minus_trusted_s * 1.0e9:.2f} ns. "
|
||||
"That remainder is the model's configured four-PCLK capture pipeline, "
|
||||
"not a measured board delay.",
|
||||
"- Production firmware and this analysis both define `POSTPONE_TOW` "
|
||||
"as 180 ms for the current RTL delivery semantics.",
|
||||
"",
|
||||
"## Hardware data used by this synthetic sweep",
|
||||
"",
|
||||
f"- Real GPS capture: {len(gdop)} GDOP samples and {len(tracked)} "
|
||||
"tracked-satellite samples over about five minutes.",
|
||||
f"- GDOP min/median/p95/max: {_fmt(min(gdop))} / "
|
||||
f"{_fmt(statistics.median(gdop))} / {_fmt(_percentile(gdop, 0.95))} / "
|
||||
f"{_fmt(max(gdop))}.",
|
||||
f"- Tracked satellites: {min(tracked)}..{max(tracked)} "
|
||||
f"(median {statistics.median(tracked):.0f}).",
|
||||
f"- Horizontal scatter RMS/p95/max: "
|
||||
f"{_fmt(telemetry.horizontal_rms_m)} / "
|
||||
f"{_fmt(telemetry.horizontal_p95_m)} / "
|
||||
f"{_fmt(telemetry.horizontal_max_m)} m. No surveyed truth was recorded.",
|
||||
"- The older fusion capture used by this sweep has no ABS_SAMPLE, "
|
||||
"REM_CODE_PHASE, TDOP/HDOP, C/N0, pseudorange residual, PPS phase/rate/"
|
||||
"integrator, temperature, or trusted-edge TIC samples. Those columns "
|
||||
"in the companion CSV are explicitly synthetic. Newer PPS status "
|
||||
"captures are analyzed separately by `hardware_replay.py` and are not "
|
||||
"silently mixed into this historical baseline.",
|
||||
"",
|
||||
"Synthetic timing uses the board-calibration values already encoded "
|
||||
"in the PPS tests: TDOP 1.50, GDOP 2.92, clock sigma 15.87 m "
|
||||
"(52.94 ns), C/N0 40.3/34.8 dB-Hz, drift noise 3.47 m/s, seven-satellite "
|
||||
"reference point, -291.75 m/s datum range rate, and 1.592 ppm local "
|
||||
"oscillator. Real GDOP and satellite count replace the geometry fields "
|
||||
"when available.",
|
||||
"",
|
||||
"## Current-loop results",
|
||||
"",
|
||||
"`period max` is the worst |edge[n]-edge[n-1]| across all seeds. "
|
||||
"`absolute p-p` and `10 s wander p-p` expose the slow left/right "
|
||||
"motion that period RMS hides.",
|
||||
"",
|
||||
"| scenario | abs RMS median (ns) | abs p-p median (ns) | "
|
||||
"10 s wander p-p (ns) | period RMS (ns) | period max (ns) | unlocks |",
|
||||
"|---|---:|---:|---:|---:|---:|---:|",
|
||||
]
|
||||
for item in current:
|
||||
lines.append(
|
||||
f"| {item.scenario} | {item.absolute_rms_median_ns:.2f} | "
|
||||
f"{item.absolute_peak_to_peak_median_ns:.2f} | "
|
||||
f"{item.wander_10s_peak_to_peak_median_ns:.2f} | "
|
||||
f"{item.period_rms_median_ns:.2f} | "
|
||||
f"{item.period_max_worst_ns:.2f} | {item.unlocks_total} |"
|
||||
)
|
||||
lines.extend(
|
||||
[
|
||||
"",
|
||||
"The loop does not numerically oscillate or lose lock in these "
|
||||
"runs. The scope-like symptom is low-frequency phase wander: it "
|
||||
"can be tens of nanoseconds while one-second period jitter remains "
|
||||
"below 20 ns. Therefore a `p95 period jitter < 20 ns` assertion is "
|
||||
"not sufficient for an absolute ±20 ns requirement.",
|
||||
"",
|
||||
"## Linear stability",
|
||||
"",
|
||||
"The fixed-point coefficients give the following discrete poles "
|
||||
f"at the {update_period_s * 1000.0:.0f} ms navigation update rate:",
|
||||
"",
|
||||
"| profile | natural frequency | damping | poles | max magnitude |",
|
||||
"|---|---:|---:|---|---:|",
|
||||
]
|
||||
)
|
||||
profile_rows = [("acquire", ACQUIRE_PROFILE)] + [
|
||||
(name, PROFILES[profile])
|
||||
for name, profile in (
|
||||
("precision", Profile.PRECISION),
|
||||
("robust", Profile.ROBUST),
|
||||
)
|
||||
]
|
||||
for name, profile in profile_rows:
|
||||
poles = discrete_poles(profile, update_period_s)
|
||||
pole_text = ", ".join(
|
||||
f"{pole.real:.6f}{pole.imag:+.6f}j" for pole in poles
|
||||
)
|
||||
lines.append(
|
||||
f"| {name} | {profile.natural_frequency_hz:.3f} Hz | "
|
||||
f"{profile.damping:.2f} | {pole_text} | "
|
||||
f"{max(abs(pole) for pole in poles):.6f} |"
|
||||
)
|
||||
lines.extend(
|
||||
[
|
||||
"",
|
||||
"All magnitudes are below one. Profile changes preserve the PI "
|
||||
"integrator, and the RTL snapshots Kp/Ki/max-slew/lock limits "
|
||||
"together with the next commit, so the current acquire/precision/"
|
||||
"robust switching is structurally sound. Stable individual modes "
|
||||
"do not prove arbitrary-switching stability, but the baseline "
|
||||
"8-update upgrade / 3-update downgrade hysteresis and the stress "
|
||||
"runs show no coefficient chatter or unlock. Hysteresis and "
|
||||
"settling counts are scaled to preserve their wall-clock duration "
|
||||
"with at least two/three independent updates.",
|
||||
"",
|
||||
"## Dynamic parameter sweep",
|
||||
"",
|
||||
"| tuning | worst period max (ns) | median abs RMS over scenarios "
|
||||
"(ns) | transitions | unlocks |",
|
||||
"|---|---:|---:|---:|---:|",
|
||||
]
|
||||
)
|
||||
for tuning in tuning_matrix():
|
||||
subset = [item for item in results if item.tuning == tuning.name]
|
||||
median_scenario_rms = statistics.median(
|
||||
item.absolute_rms_median_ns for item in subset
|
||||
)
|
||||
lines.append(
|
||||
f"| {tuning.name} | "
|
||||
f"{max(item.period_max_worst_ns for item in subset):.2f} | "
|
||||
f"{median_scenario_rms:.2f} | "
|
||||
f"{sum(item.profile_transitions_total for item in subset)} | "
|
||||
f"{sum(item.unlocks_total for item in subset)} |"
|
||||
)
|
||||
current_transition = next(
|
||||
item for item in current if item.scenario == "quality_transitions"
|
||||
)
|
||||
guarded_transition = next(
|
||||
item for item in guarded if item.scenario == "quality_transitions"
|
||||
)
|
||||
scheduled = [
|
||||
item for item in results
|
||||
if item.tuning == "quality_scheduled_drift"
|
||||
]
|
||||
lines.extend(
|
||||
[
|
||||
"",
|
||||
"The safest small candidate is `robust_noise_guard`: retain the "
|
||||
"current 0.035 Hz precision loop and 0.012 Hz robust loop, but "
|
||||
"reduce only the robust-profile WLS residual gain from 1/32 to "
|
||||
"1/64. In the synthetic quality-transition case its worst period "
|
||||
f"step changed from {current_transition.period_max_worst_ns:.2f} "
|
||||
f"to {guarded_transition.period_max_worst_ns:.2f} ns. This is a "
|
||||
"candidate for hardware A/B testing, not enough evidence for a "
|
||||
"production constant change.",
|
||||
"",
|
||||
"The best synthetic all-scenario period result is "
|
||||
"`quality_scheduled_drift`: use drift gain 1/2 only with a "
|
||||
"precision-quality Doppler solution, fall back to the current 1/4 "
|
||||
"gain in robust quality, and combine that with the 1/64 robust "
|
||||
"bias gain. Its worst period step was "
|
||||
f"{max(item.period_max_worst_ns for item in scheduled):.2f} ns "
|
||||
"with no unlocks. It is the most meaningful dynamic A/B candidate, "
|
||||
"but the repository has no real drift-covariance or TIC samples "
|
||||
"with which to validate the threshold.",
|
||||
"",
|
||||
"Narrowing precision to 0.012 Hz improves stationary noise but "
|
||||
"lags even the explicit 0.05 ppm/120 s synthetic oscillator "
|
||||
"variation. Widening it improves oscillator tracking but passes "
|
||||
"more WLS/multipath motion. A geometry-only bandwidth scheduler "
|
||||
"cannot resolve that tradeoff.",
|
||||
"",
|
||||
"## Recommended architecture update",
|
||||
"",
|
||||
"1. Keep current acquire/precision/robust PI profiles until real "
|
||||
"TIC and oscillator-temperature data are available. Do not tune "
|
||||
"from scope appearance alone.",
|
||||
"2. Log the existing firmware status line at every solve during "
|
||||
"validation: ABS_SAMPLE, REM_CODE_PHASE, capture sequence/counter, "
|
||||
"TDOP/HDOP/GDOP, C/N0, pseudorange residual RMS, raw/filtered bias, "
|
||||
"innovation, bias drift, hardware phase, rate, integrator, profile, "
|
||||
"lock, FPGA/oscillator temperature, DUT edge and trusted edge.",
|
||||
"3. A/B test the simulated scheduled filter first: precision "
|
||||
"drift gain 1/2, robust drift gain 1/4, precision bias gain 1/32, "
|
||||
"and robust bias gain 1/64. Longer term, use estimated phase/drift "
|
||||
"covariance to interpolate those software gains within bounds. "
|
||||
"Keep RTL PI bandwidth changes hysteretic and preserve its "
|
||||
"integrator as today.",
|
||||
"4. If temperature/frequency acceleration is the main source, add "
|
||||
"oscillator temperature feed-forward or a phase/frequency/frequency-"
|
||||
"drift estimator. Switching PI bandwidth alone either passes WLS "
|
||||
"wander or lags the oscillator.",
|
||||
"5. Define acceptance separately: calibrated mean offset, absolute "
|
||||
"centered error/p-p wander, one-second period error, holdover, and "
|
||||
"re-lock. A physical max-20 ns claim requires a common-clock TIC "
|
||||
"run after cable/output-delay calibration; this synthetic test "
|
||||
"cannot certify it.",
|
||||
"",
|
||||
]
|
||||
)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
path.write_text("\n".join(lines), encoding="utf-8")
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--duration", type=float, default=300.0)
|
||||
parser.add_argument("--seeds", type=int, default=24)
|
||||
parser.add_argument(
|
||||
"--update-period-ms",
|
||||
type=float,
|
||||
default=UPDATE_PERIOD_S * 1000.0,
|
||||
help="NAV solve period (production-safe examples: 200, 500, 1000)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--output-dir",
|
||||
type=Path,
|
||||
default=Path(__file__).resolve().parent / "output/stability",
|
||||
)
|
||||
parser.add_argument("--no-write", action="store_true")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
if args.duration < 120.0 or args.seeds < 1:
|
||||
raise SystemExit("duration must be >= 120 s and seeds must be positive")
|
||||
update_period_s = args.update_period_ms / 1000.0
|
||||
nav_ticks = round(update_period_s / NAV_BIT_PERIOD_S)
|
||||
if (
|
||||
not math.isfinite(update_period_s)
|
||||
or update_period_s <= 0.0
|
||||
or abs(update_period_s - nav_ticks * NAV_BIT_PERIOD_S) > 1.0e-12
|
||||
or nav_ticks <= 0
|
||||
or 300 % nav_ticks != 0
|
||||
):
|
||||
raise SystemExit(
|
||||
"update period must be N*20 ms with positive N dividing "
|
||||
"the 300-bit subframe"
|
||||
)
|
||||
software_dir = Path(__file__).resolve().parents[1]
|
||||
telemetry = load_real_telemetry(software_dir)
|
||||
if not telemetry.gdop or not telemetry.tracked:
|
||||
raise SystemExit("real GPS GDOP/satellite capture was not found")
|
||||
|
||||
epoch_result = run_end_to_end(
|
||||
EndToEndConfig(
|
||||
decoder_mode=DecoderMode.CURRENT_RTL,
|
||||
postpone_tow_s=0.180,
|
||||
)
|
||||
)
|
||||
analysis_start = max(60.0, args.duration / 3.0)
|
||||
results: list[Aggregate] = []
|
||||
telemetry_epochs: list[SyntheticEpoch] = []
|
||||
for scenario in scenario_matrix():
|
||||
seed_epochs = [
|
||||
synthesize_epochs(
|
||||
telemetry,
|
||||
scenario,
|
||||
duration_s=args.duration,
|
||||
seed=seed,
|
||||
update_period_s=update_period_s,
|
||||
)
|
||||
for seed in range(1, args.seeds + 1)
|
||||
]
|
||||
if scenario.name == "scope_like_multipath":
|
||||
telemetry_epochs = seed_epochs[0]
|
||||
for tuning in tuning_matrix():
|
||||
runs = [
|
||||
measure_trace(
|
||||
_run_loop(
|
||||
epochs,
|
||||
tuning,
|
||||
update_period_s=update_period_s,
|
||||
),
|
||||
after_s=analysis_start,
|
||||
)
|
||||
for epochs in seed_epochs
|
||||
]
|
||||
results.append(aggregate(scenario, tuning, runs))
|
||||
|
||||
print(
|
||||
"scenario tuning abs_rms "
|
||||
"abs_pp wander10 period_rms period_max unlock"
|
||||
)
|
||||
for item in results:
|
||||
print(
|
||||
f"{item.scenario:27s} {item.tuning:22s} "
|
||||
f"{item.absolute_rms_median_ns:7.2f} "
|
||||
f"{item.absolute_peak_to_peak_median_ns:7.2f} "
|
||||
f"{item.wander_10s_peak_to_peak_median_ns:8.2f} "
|
||||
f"{item.period_rms_median_ns:10.2f} "
|
||||
f"{item.period_max_worst_ns:10.2f} "
|
||||
f"{item.unlocks_total:6d}"
|
||||
)
|
||||
print(
|
||||
"current RTL + 180 ms: DUT-trusted = "
|
||||
f"{epoch_result.pps.edges[0].dut_minus_trusted_s * 1.0e9:.2f} ns"
|
||||
)
|
||||
if not args.no_write:
|
||||
write_telemetry(args.output_dir / "synthetic_telemetry.csv", telemetry_epochs)
|
||||
write_sweep(args.output_dir / "parameter_sweep.csv", results)
|
||||
write_report(
|
||||
args.output_dir / "stability_report.md",
|
||||
telemetry,
|
||||
results,
|
||||
epoch_result,
|
||||
update_period_s,
|
||||
)
|
||||
print(f"wrote {args.output_dir}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,147 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import math
|
||||
from pathlib import Path
|
||||
import tempfile
|
||||
import unittest
|
||||
|
||||
from pps_python.adaptive_pps import (
|
||||
PROFILES,
|
||||
Profile,
|
||||
Q32,
|
||||
TRACKING_HZ,
|
||||
)
|
||||
|
||||
from pps_end_to_end_validation.hardware_replay import (
|
||||
UPDATE_PERIOD_S,
|
||||
expand_samples,
|
||||
load_firmware_telemetry,
|
||||
observed_metrics,
|
||||
replay_firmware_telemetry,
|
||||
replay_tuning_sensitivity,
|
||||
)
|
||||
|
||||
|
||||
def _status_line(
|
||||
index: int,
|
||||
*,
|
||||
update_period_s: float = UPDATE_PERIOD_S,
|
||||
log_interval_updates: int = 10,
|
||||
) -> str:
|
||||
phase_ns = 140.0 + 15.0 * math.sin(index * 0.7)
|
||||
integrator_ppm = -1.560
|
||||
kp_q46, _, _ = PROFILES[Profile.ROBUST].fixed_coefficients(
|
||||
update_period_s
|
||||
)
|
||||
phase_q32 = round(phase_ns * 1.0e-9 * TRACKING_HZ * Q32)
|
||||
proportional_q32 = (phase_q32 * kp_q46) >> 46
|
||||
rate_ppm = (
|
||||
round(integrator_ppm * 1.0e-6 * Q32) + proportional_q32
|
||||
) / Q32 * 1.0e6
|
||||
raw_bias_m = 700_000.0 - 80.0 * (2.0 * index) + 4.0 * math.sin(index)
|
||||
filtered_bias_m = 700_000.0 - 80.0 * (2.0 * index)
|
||||
return (
|
||||
"[NAV] PPS profile=robust mode=tracking locked=1 "
|
||||
"tdop=1.22 gdop=2.82 clock_sigma=11.24 cn0=42.6/38.4 "
|
||||
f"innovation={raw_bias_m - filtered_bias_m:.2f}m "
|
||||
f"raw_bias={raw_bias_m:.3f}m filtered_bias={filtered_bias_m:.3f}m "
|
||||
"clock_drift=110.000mps bias_drift=-80.000mps predictor=1 "
|
||||
"eskf_alpha=0.05 eskf_v=0.100/0.050/0x00 "
|
||||
"pps_v=0.100/0.080/0x00 ref_svid=23 capture_corr=32.00ns "
|
||||
f"hw_phase={phase_ns:.2f}ns rate_cmd={rate_ppm:.3f}ppm "
|
||||
f"integ={integrator_ppm:.3f}ppm pulse={100 + 2 * index} "
|
||||
f"seq={1000 + log_interval_updates * index} status=0x80f"
|
||||
)
|
||||
|
||||
|
||||
def _write_capture(
|
||||
path: Path,
|
||||
count: int = 16,
|
||||
*,
|
||||
update_period_s: float = UPDATE_PERIOD_S,
|
||||
log_interval_updates: int = 10,
|
||||
) -> None:
|
||||
location = (
|
||||
"[LOCATION] source=gps lat=0 lon=0 height=0 "
|
||||
"gdop=2.82 pdop=2.54 hdop=1.81 vdop=1.78 tdop=1.22 "
|
||||
"tracked=6 used=5 velocity_valid=1 velocity_fde=0 "
|
||||
"speed=0.1 velocity_rms=0.08"
|
||||
)
|
||||
with path.open("w", encoding="utf-8", newline="") as output:
|
||||
writer = csv.writer(output, lineterminator="\n")
|
||||
writer.writerow(("host_monotonic_s", "host_unix_s", "uart_line"))
|
||||
for index in range(count):
|
||||
host_s = update_period_s * log_interval_updates * index
|
||||
writer.writerow((host_s - 0.01, 1000.0 + host_s, location))
|
||||
writer.writerow((
|
||||
host_s,
|
||||
1000.01 + host_s,
|
||||
_status_line(
|
||||
index,
|
||||
update_period_s=update_period_s,
|
||||
log_interval_updates=log_interval_updates,
|
||||
),
|
||||
))
|
||||
|
||||
|
||||
class HardwareReplayTests(unittest.TestCase):
|
||||
def setUp(self) -> None:
|
||||
self.temporary = tempfile.TemporaryDirectory()
|
||||
self.capture = Path(self.temporary.name) / "capture.csv"
|
||||
_write_capture(self.capture)
|
||||
|
||||
def tearDown(self) -> None:
|
||||
self.temporary.cleanup()
|
||||
|
||||
def test_uart_parser_and_sequence_expansion(self):
|
||||
samples = load_firmware_telemetry(self.capture)
|
||||
self.assertEqual(len(samples), 16)
|
||||
self.assertEqual(samples[-1].time_s, 30.0)
|
||||
self.assertEqual(samples[0].tracked_satellites, 6)
|
||||
self.assertEqual(samples[0].used_satellites, 5)
|
||||
expanded = expand_samples(samples)
|
||||
self.assertEqual(len(expanded), 151)
|
||||
self.assertEqual(sum(interpolated for _, interpolated in expanded), 135)
|
||||
|
||||
def test_fixed_point_pi_and_checkpoint_replay(self):
|
||||
samples = load_firmware_telemetry(self.capture)
|
||||
observed = observed_metrics(samples)
|
||||
self.assertEqual(observed.locked_fraction, 1.0)
|
||||
self.assertLess(observed.pi_rate_reproduction_rms_ppb, 0.6)
|
||||
points, replay = replay_firmware_telemetry(
|
||||
samples, warmup_s=5.0
|
||||
)
|
||||
self.assertEqual(len(points), 151)
|
||||
self.assertEqual(replay.unlocks, 0)
|
||||
self.assertEqual(replay.locked_fraction_after_warmup, 1.0)
|
||||
self.assertTrue(math.isfinite(replay.modeled_edge_centered_rms_ns))
|
||||
self.assertGreater(replay.modeled_edge_centered_rms_ns, 0.0)
|
||||
|
||||
def test_one_second_cadence_is_inferred_from_uart_sequence(self):
|
||||
_write_capture(
|
||||
self.capture,
|
||||
update_period_s=1.0,
|
||||
log_interval_updates=2,
|
||||
)
|
||||
samples = load_firmware_telemetry(self.capture)
|
||||
self.assertEqual(samples[-1].time_s, 30.0)
|
||||
self.assertEqual(len(expand_samples(samples)), 31)
|
||||
observed = observed_metrics(samples)
|
||||
self.assertLess(observed.pi_rate_reproduction_rms_ppb, 0.6)
|
||||
_, replay = replay_firmware_telemetry(samples, warmup_s=5.0)
|
||||
self.assertEqual(replay.expanded_updates, 31)
|
||||
self.assertEqual(replay.unlocks, 0)
|
||||
|
||||
def test_tuning_sensitivity_keeps_candidates_separate(self):
|
||||
samples = load_firmware_telemetry(self.capture)
|
||||
results = replay_tuning_sensitivity(samples, warmup_s=5.0)
|
||||
self.assertEqual(
|
||||
[result.tuning for result in results],
|
||||
["current", "robust_bias_1_64", "drift_gain_1_2"],
|
||||
)
|
||||
self.assertTrue(all(result.unlocks == 0 for result in results))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,176 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
import re
|
||||
import unittest
|
||||
|
||||
from pps_end_to_end_validation.pps_end_to_end import (
|
||||
DecoderMode,
|
||||
EndToEndConfig,
|
||||
LNAV_WINDOW_BITS,
|
||||
encode_lnav_subframe,
|
||||
run_end_to_end,
|
||||
validate_rtl_window,
|
||||
)
|
||||
|
||||
|
||||
class LnavDecoderTests(unittest.TestCase):
|
||||
def make_window(self):
|
||||
previous_d29 = 0
|
||||
previous_d30 = 0
|
||||
current = encode_lnav_subframe(
|
||||
57_600, 1, previous_d29, previous_d30
|
||||
)
|
||||
following = encode_lnav_subframe(
|
||||
57_601,
|
||||
2,
|
||||
current.ending_d29,
|
||||
current.ending_d30,
|
||||
)
|
||||
window = (
|
||||
previous_d29,
|
||||
previous_d30,
|
||||
*current.transmitted_bits,
|
||||
*following.transmitted_bits[:8],
|
||||
)
|
||||
self.assertEqual(len(window), LNAV_WINDOW_BITS)
|
||||
return current, window
|
||||
|
||||
def test_parity_valid_subframe_round_trips_through_rtl_predicate(self):
|
||||
current, window = self.make_window()
|
||||
decoded = validate_rtl_window(window)
|
||||
self.assertIsNotNone(decoded)
|
||||
self.assertEqual(decoded.z_count, current.z_count)
|
||||
self.assertEqual(decoded.subframe_id, current.subframe_id)
|
||||
self.assertEqual(decoded.normalized_words, current.normalized_words)
|
||||
|
||||
def test_single_data_bit_corruption_prevents_subframe_interrupt(self):
|
||||
_, window = self.make_window()
|
||||
corrupted = list(window)
|
||||
corrupted[2 + 73] ^= 1
|
||||
self.assertIsNone(validate_rtl_window(corrupted))
|
||||
|
||||
|
||||
class PpsEndToEndTests(unittest.TestCase):
|
||||
def test_model_default_matches_production_firmware_postpone_tow(self):
|
||||
header = (
|
||||
Path(__file__).resolve().parents[1]
|
||||
/ "gps_system/header/gps/gps_ip_defines.h"
|
||||
).read_text(encoding="utf-8")
|
||||
match = re.search(
|
||||
r"#define\s+POSTPONE_TOW\s+\(double\)\(([0-9.]+)\)",
|
||||
header,
|
||||
)
|
||||
self.assertIsNotNone(match)
|
||||
self.assertAlmostEqual(float(match.group(1)), 0.180)
|
||||
self.assertAlmostEqual(
|
||||
EndToEndConfig().postpone_tow_s,
|
||||
float(match.group(1)),
|
||||
)
|
||||
|
||||
def test_bit_generator_recreates_50bps_stream_and_registered_sample_lag(self):
|
||||
result = run_end_to_end(EndToEndConfig())
|
||||
self.assertEqual(result.bit_valid_events, 2 + 3 * 300)
|
||||
self.assertAlmostEqual(result.bit_generator_sample_lag_ms, 1.0, places=6)
|
||||
|
||||
def test_current_rtl_validates_on_ninth_following_bit(self):
|
||||
result = run_end_to_end(EndToEndConfig())
|
||||
self.assertEqual(result.first_subframe_readout_bits, 9)
|
||||
self.assertEqual(result.first_nav_solve_bit, 19)
|
||||
self.assertGreaterEqual(result.subframe_interrupts, 2)
|
||||
self.assertGreater(result.nav_solve_interrupts, 20)
|
||||
self.assertEqual(result.firmware_rejected_nav_solves, 0)
|
||||
|
||||
def test_non_subframe_divisor_explains_40_bit_build_restriction(self):
|
||||
# N=40 divides the GPS week but not a 300-bit subframe. Refreshing the
|
||||
# HOW sample anchor changes timer phase by half a solve period, so the
|
||||
# model rejects solves. Firmware now prevents this configuration.
|
||||
self.assertEqual(30_240_000 % 40, 0)
|
||||
self.assertNotEqual(300 % 40, 0)
|
||||
result = run_end_to_end(
|
||||
EndToEndConfig(nav_solve_period_bits=40, subframes=4)
|
||||
)
|
||||
self.assertGreater(result.firmware_rejected_nav_solves, 0)
|
||||
|
||||
def test_supported_200_500_1000ms_periods_keep_absolute_epoch(self):
|
||||
for period_bits in (10, 25, 50):
|
||||
result = run_end_to_end(
|
||||
EndToEndConfig(
|
||||
nav_solve_period_bits=period_bits,
|
||||
subframes=4,
|
||||
)
|
||||
)
|
||||
self.assertEqual(result.firmware_rejected_nav_solves, 0)
|
||||
self.assertAlmostEqual(
|
||||
result.expected_semantic_offset_ms, 0.0, places=9
|
||||
)
|
||||
self.assertLess(
|
||||
abs(result.pps.edges[0].dut_minus_trusted_s),
|
||||
100.0e-9,
|
||||
)
|
||||
|
||||
def test_current_rtl_and_160ms_correction_make_pps_20ms_late(self):
|
||||
result = run_end_to_end(EndToEndConfig(postpone_tow_s=0.160))
|
||||
offset_s = result.pps.edges[0].dut_minus_trusted_s
|
||||
self.assertAlmostEqual(result.expected_semantic_offset_ms, 20.0, places=9)
|
||||
self.assertGreater(offset_s, 0.019999)
|
||||
self.assertLess(offset_s, 0.020001)
|
||||
for edge in result.pps.edges:
|
||||
self.assertAlmostEqual(edge.dut_minus_trusted_s, offset_s, places=9)
|
||||
|
||||
def test_current_bit_validation_and_160ms_remove_lnav_bit_error(self):
|
||||
result = run_end_to_end(
|
||||
EndToEndConfig(
|
||||
decoder_mode=DecoderMode.CURRENT_BIT,
|
||||
postpone_tow_s=0.160,
|
||||
)
|
||||
)
|
||||
self.assertEqual(result.first_subframe_readout_bits, 8)
|
||||
self.assertEqual(result.first_nav_solve_bit, 18)
|
||||
self.assertAlmostEqual(result.expected_semantic_offset_ms, 0.0, places=9)
|
||||
self.assertLess(abs(result.pps.edges[0].dut_minus_trusted_s), 100.0e-9)
|
||||
|
||||
def test_180ms_production_firmware_matches_current_rtl(self):
|
||||
result = run_end_to_end(EndToEndConfig())
|
||||
self.assertEqual(result.first_subframe_readout_bits, 9)
|
||||
self.assertAlmostEqual(result.expected_semantic_offset_ms, 0.0, places=9)
|
||||
self.assertLess(abs(result.pps.edges[0].dut_minus_trusted_s), 100.0e-9)
|
||||
|
||||
def test_timestamped_commit_rejects_cpu_delay_as_phase(self):
|
||||
fast = run_end_to_end(
|
||||
EndToEndConfig(firmware_compute_delay_s=0.001)
|
||||
)
|
||||
stalled = run_end_to_end(
|
||||
EndToEndConfig(firmware_compute_delay_s=0.080)
|
||||
)
|
||||
self.assertAlmostEqual(
|
||||
fast.pps.edges[0].dut_minus_trusted_s,
|
||||
stalled.pps.edges[0].dut_minus_trusted_s,
|
||||
places=9,
|
||||
)
|
||||
self.assertGreater(stalled.pps.commit_age_s, fast.pps.commit_age_s)
|
||||
|
||||
def test_vendor_and_dut_fixed_delays_have_expected_sign(self):
|
||||
result = run_end_to_end(
|
||||
EndToEndConfig(
|
||||
decoder_mode=DecoderMode.CURRENT_BIT,
|
||||
postpone_tow_s=0.160,
|
||||
dut_output_delay_s=12.0e-9,
|
||||
trusted_output_delay_s=35.0e-9,
|
||||
)
|
||||
)
|
||||
baseline = run_end_to_end(
|
||||
EndToEndConfig(
|
||||
decoder_mode=DecoderMode.CURRENT_BIT,
|
||||
postpone_tow_s=0.160,
|
||||
)
|
||||
)
|
||||
delta = (
|
||||
result.pps.edges[0].dut_minus_trusted_s
|
||||
- baseline.pps.edges[0].dut_minus_trusted_s
|
||||
)
|
||||
self.assertAlmostEqual(delta, -23.0e-9, places=10)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,118 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from pathlib import Path
|
||||
import unittest
|
||||
|
||||
from pps_python.adaptive_pps import ACQUIRE_PROFILE, PROFILES, Profile
|
||||
|
||||
from pps_end_to_end_validation.stability_analysis import (
|
||||
discrete_poles,
|
||||
load_real_telemetry,
|
||||
measure_trace,
|
||||
scenario_matrix,
|
||||
synthesize_epochs,
|
||||
tuning_matrix,
|
||||
_run_loop,
|
||||
)
|
||||
|
||||
|
||||
SOFTWARE_DIR = Path(__file__).resolve().parents[1]
|
||||
|
||||
|
||||
class StabilityAnalysisTests(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
cls.telemetry = load_real_telemetry(SOFTWARE_DIR)
|
||||
|
||||
def test_repository_capture_is_loaded(self):
|
||||
self.assertEqual(len(self.telemetry.gdop), 989)
|
||||
self.assertEqual(len(self.telemetry.tracked), 990)
|
||||
self.assertEqual(set(self.telemetry.tracked), {8})
|
||||
self.assertAlmostEqual(min(self.telemetry.gdop), 1.8593929921)
|
||||
self.assertAlmostEqual(max(self.telemetry.gdop), 3.1537716348)
|
||||
self.assertGreater(self.telemetry.horizontal_rms_m, 8.0)
|
||||
self.assertLess(self.telemetry.horizontal_rms_m, 9.0)
|
||||
|
||||
def test_fixed_point_profiles_have_inside_unit_circle_poles(self):
|
||||
for profile in (
|
||||
ACQUIRE_PROFILE,
|
||||
PROFILES[Profile.PRECISION],
|
||||
PROFILES[Profile.ROBUST],
|
||||
):
|
||||
baseline_poles = discrete_poles(profile)
|
||||
self.assertTrue(all(abs(pole) < 1.0 for pole in baseline_poles))
|
||||
for period_s in (0.5, 1.0):
|
||||
mapped_poles = discrete_poles(profile, period_s)
|
||||
expected_poles = tuple(
|
||||
pole ** (period_s / 0.2) for pole in baseline_poles
|
||||
)
|
||||
self.assertTrue(all(abs(pole) < 1.0 for pole in mapped_poles))
|
||||
self.assertLess(
|
||||
max(
|
||||
abs(actual - expected)
|
||||
for actual, expected in zip(
|
||||
mapped_poles, expected_poles
|
||||
)
|
||||
),
|
||||
1.0e-4,
|
||||
)
|
||||
|
||||
def test_synthetic_telemetry_exercises_wrap_and_physical_fields(self):
|
||||
epochs = synthesize_epochs(
|
||||
self.telemetry,
|
||||
scenario_matrix()[0],
|
||||
duration_s=120.0,
|
||||
seed=1,
|
||||
)
|
||||
self.assertEqual(len(epochs), 600)
|
||||
self.assertTrue(any(epochs[i].abs_sample < epochs[i - 1].abs_sample
|
||||
for i in range(1, len(epochs))))
|
||||
self.assertTrue(all(0 <= epoch.rem_code_phase <= 1_023_000
|
||||
for epoch in epochs))
|
||||
self.assertTrue(all(epoch.used_satellites == 8 for epoch in epochs))
|
||||
self.assertTrue(all(math.isfinite(epoch.corrected_abs_sample)
|
||||
for epoch in epochs))
|
||||
self.assertTrue(all(epoch.capture_correction_ns >= 0.0
|
||||
for epoch in epochs))
|
||||
|
||||
def test_current_loop_locks_without_unlock_in_board_scenario(self):
|
||||
epochs = synthesize_epochs(
|
||||
self.telemetry,
|
||||
scenario_matrix()[0],
|
||||
duration_s=120.0,
|
||||
seed=4,
|
||||
)
|
||||
trace = _run_loop(epochs, tuning_matrix()[0])
|
||||
metrics = measure_trace(trace, after_s=60.0)
|
||||
self.assertLess(metrics.first_lock_s, 15.0)
|
||||
self.assertEqual(metrics.unlocks, 0)
|
||||
self.assertLessEqual(metrics.period_max_ns, 16.0 + 1.0e-9)
|
||||
# Slow absolute motion is deliberately measured separately.
|
||||
self.assertGreater(
|
||||
metrics.absolute_peak_to_peak_ns,
|
||||
metrics.period_max_ns,
|
||||
)
|
||||
|
||||
def test_current_loop_remains_stable_at_supported_slower_cadences(self):
|
||||
for period_s in (0.5, 1.0):
|
||||
epochs = synthesize_epochs(
|
||||
self.telemetry,
|
||||
scenario_matrix()[0],
|
||||
duration_s=180.0,
|
||||
seed=4,
|
||||
update_period_s=period_s,
|
||||
)
|
||||
trace = _run_loop(
|
||||
epochs,
|
||||
tuning_matrix()[0],
|
||||
update_period_s=period_s,
|
||||
)
|
||||
metrics = measure_trace(trace, after_s=60.0)
|
||||
self.assertLess(metrics.first_lock_s, 15.0)
|
||||
self.assertEqual(metrics.unlocks, 0)
|
||||
self.assertLessEqual(metrics.period_max_ns, 16.0 + 1.0e-9)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Binary file not shown.
@@ -9,7 +9,8 @@ davranışlarını taklit eder:
|
||||
|
||||
- RF `ABS_SAMPLE` sayacı: `16.367981 MHz`, 32 bit ve wrap eden sayaç.
|
||||
- Tracking/PPS sayacı: `125 MHz` free-running sayaç.
|
||||
- Navigation solve periyodu: `NAV_SOLVE_TIMER=10`, yani `200 ms`.
|
||||
- Varsayılan navigation solve periyodu: `NAV_SOLVE_TIMER=10`, yani `200 ms`;
|
||||
period-aware loop doğrulaması ayrıca 500 ve 1000 ms'yi kapsar.
|
||||
- Ortak navigation bit transmit epoch'u ve uyduların farklı propagation
|
||||
delay'lerle art arda tracking olayı üretmesi.
|
||||
- Firmware'deki corrected sample denklemi:
|
||||
@@ -59,6 +60,28 @@ python3 pps_python/simulate_adaptive_pps.py
|
||||
python3 -m unittest pps_python/test_pps_adaptive.py
|
||||
```
|
||||
|
||||
## Bit-level Linux PPS uçtan uca testi
|
||||
|
||||
Bit üretiminden trusted PPS karşılaştırmasına kadar deterministik mutlak zaman
|
||||
regresyonu ayrı `pps_end_to_end_validation` paketine taşınmıştır. Ayrıntılı
|
||||
kapsam, 20 ms teşhisi ve donanım doğrulama adımları için
|
||||
[`pps_end_to_end_validation/README.md`](../pps_end_to_end_validation/README.md)
|
||||
dosyasına bakın.
|
||||
|
||||
Software dizininden çalıştırma:
|
||||
|
||||
```bash
|
||||
python3 -m pps_end_to_end_validation.pps_end_to_end
|
||||
python3 -m unittest pps_end_to_end_validation/test_pps_end_to_end.py
|
||||
```
|
||||
|
||||
```text
|
||||
scenario bits correction reference error DUT - trusted PPS
|
||||
current-rtl/180ms 9 180 ms -0.000032 ms +0.000032 ms
|
||||
current-rtl/160ms 9 160 ms -20.000032 ms +20.000032 ms
|
||||
current-bit/160ms 8 160 ms -0.000032 ms +0.000032 ms
|
||||
```
|
||||
|
||||
Kısa veya farklı PI koşusu:
|
||||
|
||||
```bash
|
||||
@@ -125,16 +148,31 @@ grafik bu farkı açıkça gösterir.
|
||||
|
||||
Raw `b/c` değerini PI phase error olarak doğrudan kullanmak doğru değildir.
|
||||
|
||||
Klasik GNSS denklemindeki `c*delta_t_r` clock bias ile buradaki `b` aynı datum
|
||||
değildir: capture'ların ortak receiver zamanı önceden çıkarılmıştır. Doppler
|
||||
velocity WLS'nin dördüncü state'i uydulara ortak receiver clock/frequency
|
||||
drift'idir; fakat bu göreli `b` değerinin doğrudan türevi olarak kullanılmaz.
|
||||
PPS modeli datumun değişimini seçilen referans uydunun geometrik range-rate'i,
|
||||
broadcast satellite clock drift'i ve Doppler-WLS receiver velocity'sinden
|
||||
hesaplar:
|
||||
|
||||
```text
|
||||
bias_drift =
|
||||
-reference_geometric_range_rate
|
||||
+ c * reference_satellite_clock_drift
|
||||
```
|
||||
|
||||
## İlk HOW ve PPS başlangıcı
|
||||
|
||||
`parse_subframe()` HOW Z-count alanını `6 s` ile çarpıp `tow.sec` yapar. Bu,
|
||||
uydunun transmit subframe sınırı etiketidir. Full-subframe interrupt'ının
|
||||
receiver'a ulaştığı an değildir.
|
||||
|
||||
Mevcut hardware, subframe doğrulama/parity akışında takip eden subframe'den
|
||||
8 bit daha bekler. Navigation data 50 bit/s olduğu için bu sabit readout
|
||||
gecikmesi `8 × 20 ms = 160 ms` olur ve yazılımda `POSTPONE_TOW=0.160` ile
|
||||
temsil edilir. Bu yüzden ilk teslim edilen subframe için:
|
||||
Mevcut hardware, subframe doğrulama/parity akışında eski shift register'i
|
||||
okuduğu için takip eden subframe'in dokuzuncu bitinde teslim yapar. Navigation
|
||||
data 50 bit/s olduğu için sabit readout gecikmesi `9 × 20 ms = 180 ms` olur ve
|
||||
production yazılımda `POSTPONE_TOW=0.180` ile temsil edilir. Bu yüzden ilk
|
||||
teslim edilen subframe için:
|
||||
|
||||
```text
|
||||
transmit_epoch_TOW = HOW_TOW + POSTPONE_TOW
|
||||
@@ -151,6 +189,10 @@ epoch-aligned transmit time'ı baremetal navigation katmanında tam bir kez
|
||||
oluşturmalıdır. PPS APB periferiki kendisine verilen GPS reference time'a
|
||||
`POSTPONE_TOW` değerini tekrar eklememelidir.
|
||||
|
||||
Tracking RTL ileride current-bit candidate window ile sekizinci bitte validate
|
||||
edecek şekilde düzeltilirse aynı release'de firmware değeri 160 ms'ye geri
|
||||
alınmalıdır; RTL ve firmware düzeltmeleri birbirinden bağımsız uygulanamaz.
|
||||
|
||||
İlk HOW'un hangi kanaldan geleceği sadece uydunun yakınlığına bağlı değildir.
|
||||
Acquisition tamamlanma anı, bit lock ve bir sonraki tam subframe sınırı çoğu
|
||||
zaman propagation delay farkından daha baskındır. Monte Carlo startup modeli
|
||||
@@ -159,8 +201,9 @@ bu üç etkiyi ayrı gösterir.
|
||||
Önerilen ownership:
|
||||
|
||||
1. Raw 125 MHz counter resetten itibaren free-running çalışır.
|
||||
2. İlk geçerli HOW week/6-second mutlak zaman etiketini sağlar; ilk hardware
|
||||
delivery epoch'u için 160 ms sabit readout düzeltmesi ayrıca eklenir.
|
||||
2. İlk geçerli HOW week/6-second mutlak zaman etiketini sağlar; mevcut RTL'nin
|
||||
ilk hardware delivery epoch'u için 180 ms sabit readout düzeltmesi ayrıca
|
||||
eklenir.
|
||||
3. İlk coherent WLS çözümünde minimum corrected-ABS kanalı datum olarak
|
||||
seçilir; aynı SVID mevcut kaldığı sürece onun uçuş süresi
|
||||
`STARTOFFSET - b/c` ile elde edilir.
|
||||
@@ -197,7 +240,7 @@ PI, raw 125 MHz sayacı değiştirmez. Ayrı GPS time accumulator'ın rate word'
|
||||
ayarlar:
|
||||
|
||||
```text
|
||||
integrator += Ki * phase_error * update_period
|
||||
integrator += Ki_discrete * phase_error
|
||||
rate_command = integrator + Kp * phase_error
|
||||
```
|
||||
|
||||
@@ -225,6 +268,64 @@ olması gerekir. Üretim profilleri tek seyrek WLS sıçramasında fiziksel puls
|
||||
kaçırmamak için lock'u ancak `128 tick = 1.024 us` üstünde bırakır; watchdog,
|
||||
quality holdover ve slew clamp daha büyük arızaları ayrıca sınırlar.
|
||||
|
||||
### NAV solve periyodu değiştiğinde loop dinamiği
|
||||
|
||||
`Ki` zaten ayrık-zaman katsayısıdır; aynı sayıyı 200, 500 ve 1000 ms'de
|
||||
kullanmak aynı loop değildir. Basitçe `Ki`'yi periyotla çarpmak da uzun
|
||||
periyotlarda mevcut RTL'nin gerçek kutuplarını yeterince doğru korumaz.
|
||||
|
||||
Firmware ve Python model şu uyumlu yöntemi kullanır:
|
||||
|
||||
1. Kartta doğrulanmış 200 ms Q2.46 katsayılarından RTL'nin iki ayrık kutbu
|
||||
çıkarılır.
|
||||
2. Yeni `T` periyodu için her kutup `T / 0.2` kuvvetine yükseltilir.
|
||||
3. Hedef kutuplardan, RTL'nin önce-integral-sonra-proportional commit sırasına
|
||||
uygun yeni `Kp` ve `Ki` geri hesaplanır.
|
||||
|
||||
Bu yöntem 200 ms'de eski katsayıları bit düzeyinde aynen üretir; 500 ve
|
||||
1000 ms'de aynı gerçek-zaman damping ve settling davranışını korur:
|
||||
|
||||
| NAV periyodu | profil | Kp Q2.46 | Ki Q2.46 |
|
||||
|---:|---|---:|---:|
|
||||
| 200 ms | acquire / precision / robust | 764018 / 222838 / 84891 | 64006 / 5445 / 640 |
|
||||
| 500 ms | acquire / precision / robust | 615780 / 209783 / 82980 | 127492 / 12804 / 1564 |
|
||||
| 1000 ms | acquire / precision / robust | 447388 / 190239 / 79923 | 178038 / 23149 / 3011 |
|
||||
|
||||
Toplam döngü yalnız RTL PI'den oluşmadığı için yazılım clock-bias, Doppler
|
||||
drift ve fallback drift kazançları da gerçek geçen süreyle
|
||||
`alpha_T = 1 - (1 - alpha_200ms)^(T/0.2)` biçiminde uygulanır. Quality/profile
|
||||
histerezisi, lock ve filter-settle sayaçları saniye cinsinden ölçeklenir;
|
||||
bağımsız örnek güvenliği için sırasıyla en az 2/2/3 update korunur. Reference
|
||||
watchdog `max(2 s, 3*T)` olur.
|
||||
|
||||
Desteklenen build değeri `GPS_NAV_SOLVE_TIMER_TICKS=N` için N, 300 bitlik
|
||||
LNAV subframe'i bölmelidir. Doğrulanan örnekler N=10, 25 ve 50'dir. N=40,
|
||||
HOW anchor yenilendiğinde timer fazını kaydırdığı için compile-time reddedilir.
|
||||
`POSTPONE_TOW=180 ms` NAV solve periyodundan bağımsızdır ve değişmez.
|
||||
|
||||
Örnek firmware build'leri:
|
||||
|
||||
```bash
|
||||
make -C gps_system RISCV_TOOLCHAIN=/tools/toolchains/rvtc-medany \
|
||||
GPS_NAV_SOLVE_TIMER_TICKS=10 all # 200 ms, varsayılan
|
||||
make -C gps_system RISCV_TOOLCHAIN=/tools/toolchains/rvtc-medany \
|
||||
GPS_NAV_SOLVE_TIMER_TICKS=25 all # 500 ms
|
||||
make -C gps_system RISCV_TOOLCHAIN=/tools/toolchains/rvtc-medany \
|
||||
GPS_NAV_SOLVE_TIMER_TICKS=50 all # 1000 ms
|
||||
```
|
||||
|
||||
Linux stability sweep'i aynı periyotları doğrudan kabul eder:
|
||||
|
||||
```bash
|
||||
python3 -m pps_end_to_end_validation.stability_analysis \
|
||||
--update-period-ms 500
|
||||
```
|
||||
|
||||
Kutup eşlemesi deterministik loop dinamiğini korur; daha seyrek WLS ölçümü
|
||||
aynı gürültü performansını garanti etmez. Örnek sayısı azalınca aliasing,
|
||||
outlier yakalama gecikmesi ve mutlak faz wander'ı değişebilir. Bu nedenle her
|
||||
kart/periyot kombinasyonu trusted PPS ile ayrıca ölçülmelidir.
|
||||
|
||||
## Adaptif PPS senaryo matrisi
|
||||
|
||||
Tam fixed-point/holdover matrisi:
|
||||
@@ -256,6 +357,16 @@ NaN, dört uydunun altına düşme, eşik sınırları ve histerezisi;
|
||||
`test_gps_pps_time_filter.c` drift prediction, gap/re-anchor ve hard-outlier
|
||||
reddini sınar.
|
||||
|
||||
2026-07-26 Genesys2 logu ile kalibre edilen aynı-datum A/B testinde position
|
||||
WLS clock-bias gürültüsü `15.87 m`, referans-datum drift'i yaklaşık
|
||||
`291.75 m/s`, Doppler drift gürültüsü `3.47 m/s` ve oscillator hatası
|
||||
`1.592 ppm` olarak modele verildi. Linux C fixed-point testinde Doppler
|
||||
predictor clock-bias RMS'i `2.75 m`, yalnız son bias'i tutan fallback ise
|
||||
`21.22 m` verdi. Python modeli sekiz seed'de hem phase RMS hem 125 MHz grid'e
|
||||
quantize edilmiş edge-jitter p95 için predictor'un fallback'ten iyi kalmasını
|
||||
regresyon koşulu yapar. Kart status register phase'i fiziksel pin jitter'i
|
||||
değildir; `15-20 ns` çıkış hedefi son olarak scope/TIC ile ölçülmelidir.
|
||||
|
||||
İki zaman eşiği bilinçli olarak ayrıdır: iki saniyelik watchdog gözlem yokken
|
||||
PPS validliğini holdover'a indirir; aynı referans SVID için Doppler-predicted
|
||||
clock-bias ve PI state'i 30 saniyeye kadar korunur. Modelde 15 saniyelik WLS
|
||||
@@ -279,9 +390,10 @@ yapar.
|
||||
çözümde minimum corrected ABS kanalı seçiliyor ve aynı SVID coherent
|
||||
snapshot'ta kaldığı sürece datum sabit tutuluyor. O kanalın TOW'u ve aynı
|
||||
nav-solve olayındaki C125 capture'ı birlikte taşınıyor.
|
||||
5. `POSTPONE_TOW=0.160`, hardware'ın mevcut subframe'i doğrulamak için takip
|
||||
eden subframe'den 8 bit daha beklemesinden oluşan sabit readout gecikmesidir.
|
||||
Uydu konum/saat epoch'u buna göre 160 ms ileri alınır. Bu değer signal
|
||||
5. `POSTPONE_TOW=0.180`, hardware'ın mevcut subframe'i eski shift register
|
||||
üzerinden doğrulayıp dokuzuncu takip eden bitte teslim etmesinden oluşan
|
||||
sabit readout gecikmesidir. Uydu konum/saat epoch'u buna göre 180 ms ileri
|
||||
alınır. Bu değer signal
|
||||
flight time veya WLS receiver clock bias ile birleştirilmemelidir. Mevcut
|
||||
`satpos()` zaten kullanım anında bir kez ekler; PPS peripheral tekrar eklemez.
|
||||
6. 125 MHz timestamp `ABS_SAMPLE` ve `REM_CODE_PHASE` ile aynı hardware
|
||||
|
||||
@@ -8,15 +8,17 @@ rounding, lock acquisition/release and holdover/re-acquisition.
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from dataclasses import dataclass, replace
|
||||
from enum import IntEnum
|
||||
import math
|
||||
from typing import Iterable
|
||||
from typing import Iterable, Mapping
|
||||
|
||||
|
||||
TRACKING_HZ = 125_000_000
|
||||
SPEED_OF_LIGHT_MPS = 299_792_458.0
|
||||
REFERENCE_WATCHDOG_SEC = 2.0
|
||||
BASELINE_UPDATE_PERIOD_S = 0.2
|
||||
MIN_REFERENCE_WATCHDOG_SEC = 2.0
|
||||
REFERENCE_WATCHDOG_PERIODS = 3.0
|
||||
MAX_PREDICTION_GAP_SEC = 30.0
|
||||
Q32 = 1 << 32
|
||||
Q46 = 1 << 46
|
||||
@@ -24,6 +26,27 @@ MAX_SIGNED_64 = (1 << 63) - 1
|
||||
MIN_SIGNED_64 = -(1 << 63)
|
||||
|
||||
|
||||
def _period_updates(
|
||||
baseline_updates: int,
|
||||
update_period_s: float,
|
||||
minimum_updates: int,
|
||||
) -> int:
|
||||
if not math.isfinite(update_period_s) or update_period_s <= 0.0:
|
||||
raise ValueError("update_period_s must be finite and positive")
|
||||
duration_s = baseline_updates * BASELINE_UPDATE_PERIOD_S
|
||||
return min(
|
||||
255,
|
||||
max(minimum_updates, math.ceil(duration_s / update_period_s - 1e-12)),
|
||||
)
|
||||
|
||||
|
||||
def _elapsed_gain(baseline_gain: float, elapsed_s: float) -> float:
|
||||
periods = elapsed_s / BASELINE_UPDATE_PERIOD_S
|
||||
if abs(periods - 1.0) <= 1e-12:
|
||||
return baseline_gain
|
||||
return -math.expm1(periods * math.log1p(-baseline_gain))
|
||||
|
||||
|
||||
class Profile(IntEnum):
|
||||
HOLDOVER = 0
|
||||
ROBUST = 1
|
||||
@@ -58,14 +81,74 @@ class LoopProfile:
|
||||
software_settle_updates: int
|
||||
|
||||
def fixed_coefficients(self, update_period_s: float) -> tuple[int, int, int]:
|
||||
"""Map the proven 200 ms discrete poles to ``update_period_s``.
|
||||
|
||||
The 200 ms result is exactly the legacy production coefficient set.
|
||||
At another NAV cadence each baseline pole is raised to T/0.2 before
|
||||
recovering Kp/Ki for the RTL's integrate-then-proportional ordering.
|
||||
"""
|
||||
if not math.isfinite(update_period_s) or update_period_s <= 0.0:
|
||||
raise ValueError("update_period_s must be finite and positive")
|
||||
omega = 2.0 * math.pi * self.natural_frequency_hz
|
||||
kp_q46 = round((2.0 * self.damping * omega / TRACKING_HZ) * Q46)
|
||||
ki_q46 = round(
|
||||
(omega * omega * update_period_s / TRACKING_HZ) * Q46
|
||||
baseline_kp = math.floor(
|
||||
(2.0 * self.damping * omega / TRACKING_HZ) * Q46 + 0.5
|
||||
) / Q46
|
||||
baseline_ki = math.floor(
|
||||
(
|
||||
omega * omega * BASELINE_UPDATE_PERIOD_S / TRACKING_HZ
|
||||
) * Q46 + 0.5
|
||||
) / Q46
|
||||
baseline_ticks = TRACKING_HZ * BASELINE_UPDATE_PERIOD_S
|
||||
baseline_sum = 2.0 - baseline_ticks * (
|
||||
baseline_kp + baseline_ki
|
||||
)
|
||||
baseline_product = 1.0 - baseline_ticks * baseline_kp
|
||||
discriminant = baseline_sum * baseline_sum - 4.0 * baseline_product
|
||||
period_ratio = update_period_s / BASELINE_UPDATE_PERIOD_S
|
||||
if discriminant >= 0.0:
|
||||
root = math.sqrt(discriminant)
|
||||
pole_a = 0.5 * (baseline_sum + root)
|
||||
pole_b = 0.5 * (baseline_sum - root)
|
||||
if pole_a < 0.0 or pole_b < 0.0:
|
||||
raise ValueError("baseline loop has an unmappable negative pole")
|
||||
target_a = pole_a**period_ratio
|
||||
target_b = pole_b**period_ratio
|
||||
target_sum = target_a + target_b
|
||||
target_product = target_a * target_b
|
||||
else:
|
||||
if baseline_product <= 0.0:
|
||||
raise ValueError("baseline loop has invalid complex poles")
|
||||
radius = math.sqrt(baseline_product)
|
||||
cosine = max(-1.0, min(1.0, baseline_sum / (2.0 * radius)))
|
||||
angle = math.acos(cosine)
|
||||
target_radius = radius**period_ratio
|
||||
target_sum = 2.0 * target_radius * math.cos(
|
||||
angle * period_ratio
|
||||
)
|
||||
target_product = target_radius * target_radius
|
||||
target_ticks = TRACKING_HZ * update_period_s
|
||||
kp = (1.0 - target_product) / target_ticks
|
||||
ki = (1.0 - target_sum + target_product) / target_ticks
|
||||
if not math.isfinite(kp) or not math.isfinite(ki) or kp < 0 or ki < 0:
|
||||
raise ValueError("mapped loop coefficients are invalid")
|
||||
kp_q46 = math.floor(kp * Q46 + 0.5)
|
||||
ki_q46 = math.floor(ki * Q46 + 0.5)
|
||||
max_slew_q32 = math.floor(
|
||||
self.max_slew_ppm * 1.0e-6 * Q32 + 0.5
|
||||
)
|
||||
max_slew_q32 = round(self.max_slew_ppm * 1.0e-6 * Q32)
|
||||
return kp_q46, ki_q46, max_slew_q32
|
||||
|
||||
def for_update_period(self, update_period_s: float) -> LoopProfile:
|
||||
return replace(
|
||||
self,
|
||||
lock_acquire_updates=_period_updates(
|
||||
self.lock_acquire_updates, update_period_s, 2
|
||||
),
|
||||
software_settle_updates=_period_updates(
|
||||
self.software_settle_updates, update_period_s, 3
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
PROFILES = {
|
||||
Profile.HOLDOVER: LoopProfile(
|
||||
@@ -149,10 +232,12 @@ def classify_quality(metrics: QualityMetrics) -> Profile:
|
||||
class QualitySelector:
|
||||
"""Three-profile selector with asymmetric transition hysteresis."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
def __init__(self, update_period_s: float = BASELINE_UPDATE_PERIOD_S) -> None:
|
||||
self.profile = Profile.HOLDOVER
|
||||
self.candidate = Profile.HOLDOVER
|
||||
self.candidate_updates = 0
|
||||
self.upgrade_updates = _period_updates(8, update_period_s, 2)
|
||||
self.degrade_updates = _period_updates(3, update_period_s, 2)
|
||||
|
||||
def update(self, metrics: QualityMetrics) -> tuple[Profile, bool]:
|
||||
target = classify_quality(metrics)
|
||||
@@ -165,7 +250,11 @@ class QualitySelector:
|
||||
self.candidate_updates = 1
|
||||
else:
|
||||
self.candidate_updates += 1
|
||||
required = 8 if target > self.profile else 3
|
||||
required = (
|
||||
self.upgrade_updates
|
||||
if target > self.profile
|
||||
else self.degrade_updates
|
||||
)
|
||||
if self.candidate_updates >= required:
|
||||
self.profile = target
|
||||
self.candidate_updates = 0
|
||||
@@ -242,17 +331,7 @@ class AdaptiveTrace:
|
||||
|
||||
def one_second_edge_jitter_rms(self, *, after_s: float = 0.0) -> float:
|
||||
"""Return 1 s PPS-edge jitter including the 125 MHz output grid."""
|
||||
selected = {
|
||||
round(time_s, 9): (
|
||||
round(-phase_s * TRACKING_HZ) / TRACKING_HZ
|
||||
)
|
||||
for time_s, phase_s, locked in zip(
|
||||
self.time_s, self.phase_error_s, self.locked
|
||||
)
|
||||
if locked
|
||||
and time_s >= after_s
|
||||
and abs(time_s - round(time_s)) < 1.0e-9
|
||||
}
|
||||
selected = dict(self.integer_second_edge_errors(after_s=after_s))
|
||||
differences = [
|
||||
edge_error - selected[round(time_s - 1.0, 9)]
|
||||
for time_s, edge_error in selected.items()
|
||||
@@ -264,6 +343,31 @@ class AdaptiveTrace:
|
||||
sum(value * value for value in differences) / len(differences)
|
||||
)
|
||||
|
||||
def integer_second_edge_errors(
|
||||
self,
|
||||
*,
|
||||
after_s: float = 0.0,
|
||||
locked_only: bool = True,
|
||||
) -> list[tuple[float, float]]:
|
||||
"""Return physical PPS edge error samples on the 125 MHz grid.
|
||||
|
||||
Period jitter alone hides low-frequency motion. Exposing the signed
|
||||
edge error lets stability analyses report absolute RMS, peak-to-peak
|
||||
wander and moving-average wander alongside cycle-to-cycle jitter.
|
||||
"""
|
||||
return [
|
||||
(
|
||||
round(time_s, 9),
|
||||
round(-phase_s * TRACKING_HZ) / TRACKING_HZ,
|
||||
)
|
||||
for time_s, phase_s, locked in zip(
|
||||
self.time_s, self.phase_error_s, self.locked
|
||||
)
|
||||
if (locked or not locked_only)
|
||||
and time_s >= after_s
|
||||
and abs(time_s - round(time_s)) < 1.0e-9
|
||||
]
|
||||
|
||||
def phase_rms(self, *, after_s: float = 0.0) -> float:
|
||||
values = [
|
||||
phase_s
|
||||
@@ -319,16 +423,48 @@ class FixedPointAdaptivePps:
|
||||
software_filter: bool = True,
|
||||
receiver_clock_bias_m: float = 1200.0,
|
||||
receiver_clock_drift_mps: float = 475.0,
|
||||
profiles: Mapping[Profile, LoopProfile] | None = None,
|
||||
acquire_profile: LoopProfile | None = None,
|
||||
software_drift_gain: float = 1.0 / 4.0,
|
||||
software_fallback_drift_gain: float = 1.0 / 16.0,
|
||||
) -> None:
|
||||
if not math.isfinite(update_period_s) or update_period_s <= 0.0:
|
||||
raise ValueError("update_period_s must be finite and positive")
|
||||
self.update_period_s = update_period_s
|
||||
self.oscillator_fraction = oscillator_ppm * 1.0e-6
|
||||
self.adaptive = adaptive
|
||||
self.software_filter = software_filter
|
||||
self.receiver_clock_bias_m = receiver_clock_bias_m
|
||||
self.receiver_clock_drift_mps = receiver_clock_drift_mps
|
||||
self.selector = QualitySelector()
|
||||
baseline_profiles = dict(PROFILES if profiles is None else profiles)
|
||||
self.profiles = {
|
||||
profile: parameters.for_update_period(update_period_s)
|
||||
for profile, parameters in baseline_profiles.items()
|
||||
}
|
||||
missing_profiles = set(Profile) - self.profiles.keys()
|
||||
if missing_profiles:
|
||||
raise ValueError(f"missing loop profiles: {sorted(missing_profiles)}")
|
||||
baseline_acquire_profile = (
|
||||
ACQUIRE_PROFILE if acquire_profile is None else acquire_profile
|
||||
)
|
||||
self.acquire_profile = baseline_acquire_profile.for_update_period(
|
||||
update_period_s
|
||||
)
|
||||
if not 0.0 < software_drift_gain <= 1.0:
|
||||
raise ValueError("software_drift_gain must be in (0, 1]")
|
||||
if not 0.0 < software_fallback_drift_gain <= 1.0:
|
||||
raise ValueError(
|
||||
"software_fallback_drift_gain must be in (0, 1]"
|
||||
)
|
||||
self.software_drift_gain = software_drift_gain
|
||||
self.software_fallback_drift_gain = software_fallback_drift_gain
|
||||
self.selector = QualitySelector(update_period_s)
|
||||
self.reference_watchdog_sec = max(
|
||||
MIN_REFERENCE_WATCHDOG_SEC,
|
||||
REFERENCE_WATCHDOG_PERIODS * update_period_s,
|
||||
)
|
||||
self.profile = Profile.HOLDOVER if adaptive else Profile.PRECISION
|
||||
self.parameters = PROFILES[self.profile]
|
||||
self.parameters = self.profiles[self.profile]
|
||||
self.kp_q46, self.ki_q46, self.max_slew_q32 = (
|
||||
self.parameters.fixed_coefficients(update_period_s)
|
||||
)
|
||||
@@ -370,16 +506,98 @@ class FixedPointAdaptivePps:
|
||||
def _phase_q32_to_seconds(value: int) -> float:
|
||||
return value / (TRACKING_HZ * Q32)
|
||||
|
||||
def seed_tracking_state(
|
||||
self,
|
||||
*,
|
||||
time_s: float,
|
||||
profile: Profile,
|
||||
phase_error_s: float,
|
||||
rate_command_ppm: float,
|
||||
integrator_ppm: float,
|
||||
software_error_s: float,
|
||||
software_bias_m: float,
|
||||
software_clock_drift_mps: float,
|
||||
software_previous_measurement_m: float,
|
||||
reference_id: int,
|
||||
) -> None:
|
||||
"""Start a replay from a captured, already-locked board snapshot.
|
||||
|
||||
Normal simulations deliberately start unaligned and exercise coarse
|
||||
acquisition. Hardware logs, however, are commonly captured after the
|
||||
board has been tracking for minutes. Replaying such a log from reset
|
||||
introduces an artificial acquisition transient and clears the PI
|
||||
integrator that was present on the board. This method imports only
|
||||
state that the production diagnostic registers and firmware status
|
||||
expose; subsequent updates still use the normal bit-accurate path.
|
||||
|
||||
The method must be called before any observation has been recorded.
|
||||
It is a model facility and has no firmware/RTL side effect.
|
||||
"""
|
||||
finite_values = (
|
||||
time_s,
|
||||
phase_error_s,
|
||||
rate_command_ppm,
|
||||
integrator_ppm,
|
||||
software_error_s,
|
||||
software_bias_m,
|
||||
software_clock_drift_mps,
|
||||
software_previous_measurement_m,
|
||||
)
|
||||
if self.trace.time_s:
|
||||
raise ValueError("tracking state must be seeded before replay")
|
||||
if not all(math.isfinite(value) for value in finite_values):
|
||||
raise ValueError("tracking state values must be finite")
|
||||
if time_s < 0.0:
|
||||
raise ValueError("tracking state time must be non-negative")
|
||||
if profile == Profile.HOLDOVER:
|
||||
raise ValueError("a locked snapshot cannot use holdover profile")
|
||||
|
||||
self.current_time_s = time_s
|
||||
self.raw_tick_remainder = 0.0
|
||||
self.phase_q32 = self._seconds_to_phase_q32(phase_error_s)
|
||||
self.profile = profile
|
||||
self.selector.profile = profile
|
||||
self.selector.candidate = profile
|
||||
self.selector.candidate_updates = 0
|
||||
self.aligned = True
|
||||
self.locked = True
|
||||
self.lock_good_updates = 0
|
||||
self.reference_watchdog_holdover = False
|
||||
self._select_loop_parameters()
|
||||
self.integrator_q32 = self._clamp(
|
||||
round(integrator_ppm * 1.0e-6 * Q32), self.max_slew_q32
|
||||
)
|
||||
self.rate_word_q32 = self._clamp(
|
||||
round(rate_command_ppm * 1.0e-6 * Q32), self.max_slew_q32
|
||||
)
|
||||
|
||||
self.software_error_s = software_error_s
|
||||
self.software_bias_m = software_bias_m
|
||||
self.software_clock_drift_mps = software_clock_drift_mps
|
||||
self.software_previous_measurement_m = (
|
||||
software_previous_measurement_m
|
||||
)
|
||||
self.software_drift_anchor_time_s = time_s
|
||||
self.software_drift_anchor_measurement_m = (
|
||||
software_previous_measurement_m
|
||||
)
|
||||
self.software_drift_initialized = True
|
||||
self.software_updates = max(
|
||||
1, self.parameters.software_settle_updates
|
||||
)
|
||||
self.software_reference_id = reference_id
|
||||
self.last_observation_time_s = time_s
|
||||
|
||||
def _apply_profile(self, profile: Profile) -> None:
|
||||
self.profile = profile
|
||||
self._select_loop_parameters()
|
||||
|
||||
def _select_loop_parameters(self) -> None:
|
||||
parameters = (
|
||||
ACQUIRE_PROFILE
|
||||
self.acquire_profile
|
||||
if self.adaptive and
|
||||
self.profile != Profile.HOLDOVER and not self.locked
|
||||
else PROFILES[self.profile]
|
||||
else self.profiles[self.profile]
|
||||
)
|
||||
if parameters == self.parameters:
|
||||
return
|
||||
@@ -536,20 +754,30 @@ class FixedPointAdaptivePps:
|
||||
return False, False
|
||||
|
||||
innovation_m = max(-300.0, min(300.0, innovation_m))
|
||||
effective_bias_gain = _elapsed_gain(
|
||||
self.parameters.software_bias_gain, interval_s
|
||||
)
|
||||
effective_drift_gain = _elapsed_gain(
|
||||
self.software_drift_gain, interval_s
|
||||
)
|
||||
effective_fallback_drift_gain = _elapsed_gain(
|
||||
self.software_fallback_drift_gain, interval_s
|
||||
)
|
||||
self.software_bias_m = (
|
||||
predicted_bias_m
|
||||
+ self.parameters.software_bias_gain * innovation_m
|
||||
+ effective_bias_gain * innovation_m
|
||||
)
|
||||
if observation.clock_drift_valid:
|
||||
self.software_clock_drift_mps += 0.25 * (
|
||||
self.software_clock_drift_mps += effective_drift_gain * (
|
||||
measured_drift_mps - self.software_clock_drift_mps
|
||||
)
|
||||
else:
|
||||
if self.software_updates < self.parameters.software_settle_updates:
|
||||
self.software_clock_drift_mps = fallback_drift_mps
|
||||
else:
|
||||
self.software_clock_drift_mps += (1.0 / 16.0) * (
|
||||
fallback_drift_mps - self.software_clock_drift_mps
|
||||
self.software_clock_drift_mps += (
|
||||
effective_fallback_drift_gain *
|
||||
(fallback_drift_mps - self.software_clock_drift_mps)
|
||||
)
|
||||
self.software_previous_measurement_m = measured_bias_m
|
||||
self.software_error_s = -(
|
||||
@@ -614,7 +842,7 @@ class FixedPointAdaptivePps:
|
||||
if (
|
||||
self.last_observation_time_s is None
|
||||
or observation.time_s - self.last_observation_time_s
|
||||
> REFERENCE_WATCHDOG_SEC
|
||||
> self.reference_watchdog_sec
|
||||
):
|
||||
self.reference_watchdog_holdover = True
|
||||
self._record(observation, 0, 0)
|
||||
@@ -630,7 +858,7 @@ class FixedPointAdaptivePps:
|
||||
if (
|
||||
self.last_observation_time_s is None
|
||||
or observation.time_s - self.last_observation_time_s
|
||||
> REFERENCE_WATCHDOG_SEC
|
||||
> self.reference_watchdog_sec
|
||||
):
|
||||
self.reference_watchdog_holdover = True
|
||||
self._record(observation, 0, 0)
|
||||
@@ -700,18 +928,28 @@ def run_observations(
|
||||
observations: Iterable[Observation],
|
||||
*,
|
||||
finish_time_s: float,
|
||||
update_period_s: float = BASELINE_UPDATE_PERIOD_S,
|
||||
oscillator_ppm: float = 1.65,
|
||||
adaptive: bool = True,
|
||||
software_filter: bool = True,
|
||||
receiver_clock_bias_m: float = 1200.0,
|
||||
receiver_clock_drift_mps: float = 475.0,
|
||||
profiles: Mapping[Profile, LoopProfile] | None = None,
|
||||
acquire_profile: LoopProfile | None = None,
|
||||
software_drift_gain: float = 1.0 / 4.0,
|
||||
software_fallback_drift_gain: float = 1.0 / 16.0,
|
||||
) -> AdaptiveTrace:
|
||||
model = FixedPointAdaptivePps(
|
||||
update_period_s=update_period_s,
|
||||
oscillator_ppm=oscillator_ppm,
|
||||
adaptive=adaptive,
|
||||
software_filter=software_filter,
|
||||
receiver_clock_bias_m=receiver_clock_bias_m,
|
||||
receiver_clock_drift_mps=receiver_clock_drift_mps,
|
||||
profiles=profiles,
|
||||
acquire_profile=acquire_profile,
|
||||
software_drift_gain=software_drift_gain,
|
||||
software_fallback_drift_gain=software_fallback_drift_gain,
|
||||
)
|
||||
for observation in observations:
|
||||
model.update(observation)
|
||||
|
||||
@@ -338,7 +338,7 @@ def page_executive(report: Report) -> None:
|
||||
ax,
|
||||
0.55,
|
||||
r"T_{GPS,k}=T_{TX,k}+S-\frac{b_k}{c}",
|
||||
"Minimum corrected-ABS kanalının receive GPS zamanı; ilk delivery için TTX,k = HOW + 0.160 s",
|
||||
"Minimum corrected-ABS kanalının receive GPS zamanı; ilk delivery için TTX,k = HOW + 0.180 s",
|
||||
)
|
||||
report.equation(
|
||||
ax,
|
||||
@@ -455,7 +455,7 @@ def page_lnav_timeline(report: Report) -> None:
|
||||
ax.text(x + 0.039, 0.58, label, color="white" if index < 2 else INK, ha="center", va="center", fontsize=7.5, weight="bold" if index < 2 else "normal")
|
||||
report.arrow(ax, (0.205, 0.545), (0.86, 0.49), color=ORANGE, text="HOW Z-count: sonraki 6 s subframe sınırının transmit-time etiketi", rad=-0.12)
|
||||
ax.add_patch(FancyBboxPatch((0.18, 0.445), 0.64, 0.045, boxstyle="round,pad=0.006", facecolor=PALE_ORANGE, edgecolor=ORANGE))
|
||||
ax.text(0.50, 0.467, "Hardware takip eden subframe'den 8 bit bekler: 8 × 20 ms = POSTPONE_TOW = 160 ms", fontsize=7.6, color=ORANGE, ha="center", va="center", weight="bold")
|
||||
ax.text(0.50, 0.467, "Mevcut hardware 9. takip eden bitte teslim eder: 9 × 20 ms = POSTPONE_TOW = 180 ms", fontsize=7.6, color=ORANGE, ha="center", va="center", weight="bold")
|
||||
|
||||
report.box(
|
||||
ax,
|
||||
@@ -518,7 +518,7 @@ def page_tx_rx(report: Report) -> None:
|
||||
color=ORANGE,
|
||||
)
|
||||
report.equation(ax, 0.42, r"T_{RX,i}=T_{HOW}+\frac{\rho_i+I_i+T_i}{c}-\delta t_{SV,i}", "Aynı transmit sınırı her kanalda farklı receive timestamp üretir")
|
||||
ax.text(0.50, 0.325, r"Hardware delivery epoch: $T_{event,i}=T_{HOW}+0.160\,s+\tau_i$", fontsize=9.2, color=ORANGE, ha="center", weight="bold")
|
||||
ax.text(0.50, 0.325, r"Hardware delivery epoch: $T_{event,i}=T_{HOW}+0.180\,s+\tau_i$", fontsize=9.2, color=ORANGE, ha="center", weight="bold")
|
||||
report.box(
|
||||
ax,
|
||||
0.07,
|
||||
@@ -557,7 +557,7 @@ def page_project_map(report: Report) -> None:
|
||||
(0.54, 0.54, 0.18, 0.10, "Linked-list state", "TOW owner\nsequence + sample", LIGHT, NAVY),
|
||||
(0.28, 0.54, 0.20, 0.10, "Coherent snapshot", "25 ms window\nmodulo unwrap", PALE_GREEN, GREEN),
|
||||
(0.06, 0.54, 0.17, 0.10, "Pseudorange", "minimum +\n68.802 ms", PALE_GREEN, GREEN),
|
||||
(0.06, 0.34, 0.17, 0.10, "Satellite model", "TOW + 160 ms\nephemeris/clock", PALE_BLUE, BLUE),
|
||||
(0.06, 0.34, 0.17, 0.10, "Satellite model", "TOW + 180 ms\nephemeris/clock", PALE_BLUE, BLUE),
|
||||
(0.32, 0.34, 0.23, 0.10, "Weighted LS", "[X Y Z b]\niono + tropo + URA/el", PALE_GREEN, GREEN),
|
||||
(0.64, 0.34, 0.30, 0.10, "Bugünkü çıkış", "position log + static last_solution[4]\nclock bias dışarı taşınmıyor", "#FBEAEA", RED),
|
||||
]
|
||||
@@ -796,7 +796,7 @@ def page_counter_mapping(report: Report) -> None:
|
||||
fig, ax = report.new_page("GPS time ile 125 MHz counter mapping'i")
|
||||
ax.text(0.08, 0.84, "İlk coherent WLS epoch'u k", fontsize=11, color=NAVY, weight="bold")
|
||||
boxes = [
|
||||
(0.07, 0.68, 0.25, 0.11, "Transmit etiketi", "İlk delivery: HOW + 0.160 s\nsonra nav-solve epoch", PALE_BLUE, BLUE),
|
||||
(0.07, 0.68, 0.25, 0.11, "Transmit etiketi", "İlk delivery: HOW + 0.180 s\nsonra nav-solve epoch", PALE_BLUE, BLUE),
|
||||
(0.375, 0.68, 0.25, 0.11, "WLS flight time", "τmin = S − b/c\nall-satellite solution", PALE_GREEN, GREEN),
|
||||
(0.68, 0.68, 0.25, 0.11, "Receive reference", "T_GPS,k = T_TX,k + τmin", PALE_ORANGE, ORANGE),
|
||||
(0.375, 0.47, 0.25, 0.11, "Hardware timestamp", "C125,k\naynı ABS/REM event", PALE_BLUE, BLUE),
|
||||
@@ -1304,7 +1304,7 @@ def page_open_points(report: Report) -> None:
|
||||
fig, ax = report.new_page("Projede ölçülerek kapatılacak açık noktalar")
|
||||
points = [
|
||||
("HOW boundary semantiği", "parse_subframe Z-count × 6 yapıyor. Full-subframe interrupt, bit_gen ABS_SAMPLE ve gerçek transmit next-subframe sınırı arasındaki sabit pipeline cycle sayısı scope/ILA ile ölçülmeli."),
|
||||
("POSTPONE_TOW = 0.160", "Doğrulanmış hardware readout gecikmesidir: takip eden subframe'den 8 bit beklenir; 50 bit/s hızda 160 ms. Mevcut satpos() kullanım anında bir kez ekler; PPS peripheral tekrar eklemez."),
|
||||
("POSTPONE_TOW = 0.180", "Mevcut RTL readout gecikmesidir: doğrulama dokuzuncu takip eden bitte yapılır; 50 bit/s hızda 180 ms. Mevcut satpos() kullanım anında bir kez ekler; PPS peripheral tekrar eklemez."),
|
||||
("RF ↔ 125 MHz bağı", "Aynı PLL/reference mı, bağımsız mı? Ortak drift ve independent jitter ölçülmeli; cross-timestamp uncertainty buna göre bütçelenmeli."),
|
||||
("Cold-start WLS timing", "Dört uyduda vertical/clock korelasyonu yüksek. Coarse alignment yapılabilir; PPS quality düşük işaretlenmeli ve PI bandwidth agresif seçilmemeli."),
|
||||
("Atmosfer/multipath", "Iono/tropo modele girse de residual b ve position'a sızar. Timing-only mode veya sabit/smoothed position ileride clock çözümünü iyileştirebilir."),
|
||||
@@ -1361,7 +1361,7 @@ def page_conclusion(report: Report) -> None:
|
||||
0.80,
|
||||
0.09,
|
||||
"En önemli mimari karar",
|
||||
"İlk uydu PPS'i başlatmaz. Baremetal HOW'a 160 ms readout düzeltmesini bir kez uygular; ilk coherent WLS bu etiketi free-running 125 MHz counter'a bağlar. PPS peripheral 160 ms'yi tekrar eklemez.",
|
||||
"İlk uydu PPS'i başlatmaz. Baremetal HOW'a 180 ms readout düzeltmesini bir kez uygular; ilk coherent WLS bu etiketi free-running 125 MHz counter'a bağlar. PPS peripheral 180 ms'yi tekrar eklemez.",
|
||||
face=NAVY,
|
||||
edge=NAVY,
|
||||
title_color="#8ED6E8",
|
||||
@@ -1414,7 +1414,7 @@ def generate(output_pdf: Path) -> None:
|
||||
report,
|
||||
"İlk HOW neden PPS receive-now değildir?",
|
||||
required_plots[4],
|
||||
"Sentetik acquisition/subframe Monte Carlo: hardware delivery, takip eden subframe'in 8 biti nedeniyle HOW sınırından 160 ms ve signal flight time kadar sonradır. HOW +160 ms +68.802 ms kaba yaklaşım bırakır; WLS flight-time + paired timestamp doğru handover'dır.",
|
||||
"Sentetik acquisition/subframe Monte Carlo: mevcut hardware delivery, dokuzuncu takip eden bit nedeniyle HOW sınırından 180 ms ve signal flight time kadar sonradır. HOW +180 ms +68.802 ms kaba yaklaşım bırakır; WLS flight-time + paired timestamp doğru handover'dır.",
|
||||
)
|
||||
plot_page(
|
||||
report,
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 333 KiB After Width: | Height: | Size: 333 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 213 KiB After Width: | Height: | Size: 213 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 132 KiB After Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 130 KiB After Width: | Height: | Size: 130 KiB |
File diff suppressed because it is too large
Load Diff
@@ -4,18 +4,18 @@ epochs: 900
|
||||
raw WLS bias/c range: -5.888015 .. 0.673753 ms
|
||||
derived GPS reference RMS error: 60.522 ns
|
||||
capture-to-commit delay mean/p99: 21.600 / 31.529 ms
|
||||
first HOW used as receive-now, median phase error: -229.876 ms
|
||||
first HOW + 160 ms + 68.802 ms, median phase error: -1.074 ms
|
||||
first HOW used as receive-now, median phase error: -249.876 ms
|
||||
first HOW + 180 ms + 68.802 ms, median phase error: -1.074 ms
|
||||
4 satellites: position RMS=103.877 m, timing RMS=144.920 ns
|
||||
5 satellites: position RMS=20.194 m, timing RMS=45.603 ns
|
||||
7 satellites / drop-reacquisition: position RMS=14.626 m, timing RMS=36.888 ns
|
||||
timestamped PI: tail phase RMS=274.599 ns, |phase| p95=409.200 ns, PPS RMS=815.734 ns, rejected stale commits=0
|
||||
timestamped, PI disabled: tail phase RMS=233591.232 ns, |phase| p95=302909.699 ns, PPS RMS=181359.923 ns, rejected stale commits=0
|
||||
timestamped PI: tail phase RMS=274.256 ns, |phase| p95=410.422 ns, PPS RMS=815.642 ns, rejected stale commits=0
|
||||
timestamped, PI disabled: tail phase RMS=233589.285 ns, |phase| p95=302907.720 ns, PPS RMS=181358.257 ns, rejected stale commits=0
|
||||
untagged APB-now PI: tail phase RMS=18651473.186 ns, |phase| p95=19146158.593 ns, PPS RMS=18060541.154 ns, rejected stale commits=0
|
||||
|
||||
Interpretation:
|
||||
- bias/c is millisecond-scale because it contains the 68.802 ms synthetic datum and the minimum satellite path.
|
||||
- epoch-aligned TOW (first delivery: HOW + 160 ms) + STARTOFFSET - bias/c reconstructs GPS time at the paired timestamp.
|
||||
- epoch-aligned TOW (first delivery: HOW + 180 ms) + STARTOFFSET - bias/c reconstructs GPS time at the paired timestamp.
|
||||
- treating that old reference as valid at APB arrival converts software latency into PPS phase error.
|
||||
- timestamping removes that phase misassociation; control application delay remains only as loop delay.
|
||||
- baremetal applies the fixed 160 ms readout correction once; the PPS peripheral must not add it again.
|
||||
- baremetal applies the fixed 180 ms readout correction once; the PPS peripheral must not add it again.
|
||||
|
||||
@@ -30,7 +30,7 @@ CODE_FREQ_BASIS = 1.023e6
|
||||
START_OFFSET_MS = 68.802
|
||||
START_OFFSET_S = START_OFFSET_MS / 1000.0
|
||||
NAV_SOLVE_PERIOD_S = 10 * 0.020
|
||||
POSTPONE_TOW_S = 0.160
|
||||
POSTPONE_TOW_S = 0.180
|
||||
OMEGA_EARTH = 7.2921151467e-5
|
||||
WEEK_S = 604_800.0
|
||||
|
||||
|
||||
@@ -298,8 +298,9 @@ def simulate_startup_tow(
|
||||
acquisition_ready = rng.uniform(0.0, 18.0, size=(trial_count, len(satellites)))
|
||||
complete_subframe_tx = (np.floor(acquisition_ready / 6.0) + 2.0) * 6.0
|
||||
# The current hardware validates/delivers the completed subframe only
|
||||
# after receiving eight bits from the following subframe. At 50 bit/s
|
||||
# this is a deterministic 160 ms readout delay (POSTPONE_TOW), distinct
|
||||
# on the ninth following bit because validation reads the old shift
|
||||
# register. At 50 bit/s
|
||||
# this is a deterministic 180 ms readout delay (POSTPONE_TOW), distinct
|
||||
# from satellite signal flight time.
|
||||
how_interrupt_rx = (
|
||||
complete_subframe_tx + POSTPONE_TOW_S + flight_times[None, :]
|
||||
@@ -354,7 +355,7 @@ def plot_startup_tow(output_dir: Path, startup: dict[str, np.ndarray]) -> None:
|
||||
alpha=0.8,
|
||||
)
|
||||
axes[1, 0].set_xlabel("Phase error [ms]")
|
||||
axes[1, 0].set_title("Provisional: HOW + 160 ms + 68.802 ms")
|
||||
axes[1, 0].set_title("Provisional: HOW + 180 ms + 68.802 ms")
|
||||
axes[1, 0].grid(True, alpha=0.3)
|
||||
|
||||
axes[1, 1].hist(
|
||||
@@ -397,7 +398,7 @@ def build_summary(
|
||||
f"derived GPS reference RMS error: {finite_rms(timing_error)*1e9:.3f} ns",
|
||||
f"capture-to-commit delay mean/p99: {np.mean(latency)*1e3:.3f} / {np.percentile(latency, 99)*1e3:.3f} ms",
|
||||
f"first HOW used as receive-now, median phase error: {np.median(startup['how_as_receive_error_s'])*1e3:.3f} ms",
|
||||
f"first HOW + 160 ms + 68.802 ms, median phase error: {np.median(startup['how_plus_pipeline_startoffset_error_s'])*1e3:.3f} ms",
|
||||
f"first HOW + 180 ms + 68.802 ms, median phase error: {np.median(startup['how_plus_pipeline_startoffset_error_s'])*1e3:.3f} ms",
|
||||
]
|
||||
for start, stop, label in [
|
||||
(0.0, 20.0, "4 satellites"),
|
||||
@@ -426,10 +427,10 @@ def build_summary(
|
||||
"",
|
||||
"Interpretation:",
|
||||
"- bias/c is millisecond-scale because it contains the 68.802 ms synthetic datum and the minimum satellite path.",
|
||||
"- epoch-aligned TOW (first delivery: HOW + 160 ms) + STARTOFFSET - bias/c reconstructs GPS time at the paired timestamp.",
|
||||
"- epoch-aligned TOW (first delivery: HOW + 180 ms) + STARTOFFSET - bias/c reconstructs GPS time at the paired timestamp.",
|
||||
"- treating that old reference as valid at APB arrival converts software latency into PPS phase error.",
|
||||
"- timestamping removes that phase misassociation; control application delay remains only as loop delay.",
|
||||
"- baremetal applies the fixed 160 ms readout correction once; the PPS peripheral must not add it again.",
|
||||
"- baremetal applies the fixed 180 ms readout correction once; the PPS peripheral must not add it again.",
|
||||
]
|
||||
)
|
||||
return "\n".join(lines) + "\n"
|
||||
|
||||
@@ -6,6 +6,7 @@ import unittest
|
||||
|
||||
from pps_python.adaptive_pps import (
|
||||
ACQUIRE_PROFILE,
|
||||
FixedPointAdaptivePps,
|
||||
Observation,
|
||||
Profile,
|
||||
PROFILES,
|
||||
@@ -59,6 +60,145 @@ def noisy_observations(
|
||||
|
||||
|
||||
class AdaptivePpsTests(unittest.TestCase):
|
||||
def test_period_aware_coefficients_match_firmware(self):
|
||||
expected = {
|
||||
0.2: {
|
||||
"acquire": (764018, 64006),
|
||||
Profile.PRECISION: (222838, 5445),
|
||||
Profile.ROBUST: (84891, 640),
|
||||
},
|
||||
0.5: {
|
||||
"acquire": (615780, 127492),
|
||||
Profile.PRECISION: (209783, 12804),
|
||||
Profile.ROBUST: (82980, 1564),
|
||||
},
|
||||
1.0: {
|
||||
"acquire": (447388, 178038),
|
||||
Profile.PRECISION: (190239, 23149),
|
||||
Profile.ROBUST: (79923, 3011),
|
||||
},
|
||||
}
|
||||
for period_s, coefficients in expected.items():
|
||||
self.assertEqual(
|
||||
ACQUIRE_PROFILE.fixed_coefficients(period_s)[:2],
|
||||
coefficients["acquire"],
|
||||
)
|
||||
for profile in (Profile.PRECISION, Profile.ROBUST):
|
||||
self.assertEqual(
|
||||
PROFILES[profile].fixed_coefficients(period_s)[:2],
|
||||
coefficients[profile],
|
||||
)
|
||||
|
||||
def test_wall_clock_hysteresis_and_settling_scale_with_period(self):
|
||||
model_500ms = FixedPointAdaptivePps(update_period_s=0.5)
|
||||
self.assertEqual(model_500ms.selector.upgrade_updates, 4)
|
||||
self.assertEqual(model_500ms.selector.degrade_updates, 2)
|
||||
self.assertEqual(
|
||||
model_500ms.profiles[Profile.PRECISION].software_settle_updates,
|
||||
5,
|
||||
)
|
||||
self.assertEqual(
|
||||
model_500ms.profiles[Profile.ROBUST].lock_acquire_updates,
|
||||
4,
|
||||
)
|
||||
|
||||
model_1s = FixedPointAdaptivePps(update_period_s=1.0)
|
||||
self.assertEqual(model_1s.selector.upgrade_updates, 2)
|
||||
self.assertEqual(model_1s.selector.degrade_updates, 2)
|
||||
self.assertEqual(
|
||||
model_1s.profiles[Profile.PRECISION].software_settle_updates,
|
||||
3,
|
||||
)
|
||||
self.assertEqual(
|
||||
model_1s.profiles[Profile.ROBUST].lock_acquire_updates,
|
||||
2,
|
||||
)
|
||||
self.assertEqual(model_1s.reference_watchdog_sec, 3.0)
|
||||
|
||||
def test_board_calibrated_doppler_predictor_reduces_pps_error(self):
|
||||
"""Genesys2 WLS statistics: predictor-on must beat fallback-only."""
|
||||
quality_metrics = QualityMetrics(
|
||||
1.50, 2.92, 15.87, 40.3, 34.8, 0.35, 0.23, 7
|
||||
)
|
||||
aided_phase_rms = []
|
||||
fallback_phase_rms = []
|
||||
aided_edge_jitter = []
|
||||
fallback_edge_jitter = []
|
||||
for seed in range(8):
|
||||
rng = random.Random(7300 + seed)
|
||||
observations = []
|
||||
for update in range(900):
|
||||
time_s = 0.2 * update
|
||||
long_omega = 2.0 * math.pi / 75.0
|
||||
short_omega = 2.0 * math.pi / 19.0
|
||||
varying_drift_mps = (
|
||||
5.0 * math.sin(long_omega * time_s)
|
||||
+ 2.0 * math.sin(short_omega * time_s)
|
||||
)
|
||||
varying_bias_m = (
|
||||
5.0 * (1.0 - math.cos(long_omega * time_s)) / long_omega
|
||||
+ 2.0 * (1.0 - math.cos(short_omega * time_s))
|
||||
/ short_omega
|
||||
)
|
||||
# 15.87 m / c = 52.94 ns; 3.47 m/s is the measured
|
||||
# board standard deviation of the reference bias drift.
|
||||
noise_s = rng.gauss(0.0, 15.87 / 299_792_458.0)
|
||||
if update % 113 == 0 and update != 0:
|
||||
noise_s += rng.choice((-1.0, 1.0)) * 120.0e-9
|
||||
observations.append(
|
||||
Observation(
|
||||
time_s,
|
||||
noise_s,
|
||||
varying_drift_mps + rng.gauss(0.0, 3.47),
|
||||
quality_metrics,
|
||||
True,
|
||||
-291.75,
|
||||
1,
|
||||
varying_bias_m,
|
||||
)
|
||||
)
|
||||
aided = run_observations(
|
||||
observations,
|
||||
finish_time_s=180.0,
|
||||
oscillator_ppm=1.592,
|
||||
)
|
||||
fallback = run_observations(
|
||||
[
|
||||
Observation(
|
||||
item.time_s,
|
||||
item.timing_error_s,
|
||||
item.clock_drift_error_mps,
|
||||
item.quality,
|
||||
False,
|
||||
item.reference_range_rate_mps,
|
||||
item.reference_id,
|
||||
item.reference_datum_offset_m,
|
||||
)
|
||||
for item in observations
|
||||
],
|
||||
finish_time_s=180.0,
|
||||
oscillator_ppm=1.592,
|
||||
)
|
||||
aided_phase_rms.append(aided.phase_rms(after_s=60.0))
|
||||
fallback_phase_rms.append(fallback.phase_rms(after_s=60.0))
|
||||
aided_edge_jitter.append(
|
||||
aided.one_second_edge_jitter_rms(after_s=60.0)
|
||||
)
|
||||
fallback_edge_jitter.append(
|
||||
fallback.one_second_edge_jitter_rms(after_s=60.0)
|
||||
)
|
||||
self.assertEqual(aided.unlock_events, 0)
|
||||
|
||||
self.assertLess(
|
||||
sum(aided_phase_rms) / len(aided_phase_rms),
|
||||
0.35 * sum(fallback_phase_rms) / len(fallback_phase_rms),
|
||||
)
|
||||
self.assertLess(
|
||||
sum(aided_edge_jitter) / len(aided_edge_jitter),
|
||||
0.70 * sum(fallback_edge_jitter) / len(fallback_edge_jitter),
|
||||
)
|
||||
self.assertLess(max(aided_phase_rms), 30.0e-9)
|
||||
|
||||
def test_quality_hysteresis_rejects_metric_chatter(self):
|
||||
selector = QualitySelector()
|
||||
for _ in range(7):
|
||||
|
||||
@@ -29,17 +29,17 @@ done
|
||||
export Z3_LIB_DIR="${Z3_LIB_DIR:-$_gps_z3_dir}"
|
||||
|
||||
_gps_riscv_gcc=""
|
||||
if [[ -n "${TOOLCHAIN:-}" ]]; then
|
||||
_gps_candidates=("$TOOLCHAIN")
|
||||
elif [[ -n "${RISCV_TOOLCHAIN:-}" ]]; then
|
||||
if [[ -n "${RISCV_TOOLCHAIN:-}" ]]; then
|
||||
_gps_candidates=("$RISCV_TOOLCHAIN" "$RISCV_TOOLCHAIN/bin")
|
||||
elif [[ -n "${TOOLCHAIN:-}" ]]; then
|
||||
_gps_candidates=("$TOOLCHAIN")
|
||||
else
|
||||
_gps_candidates=(
|
||||
"$GPS_ROOT/tools/local/riscv/bin"
|
||||
/tools/toolchains-medany/bin
|
||||
/tools/toolchains/rvtc-medany/bin
|
||||
/tools/toolchain/bin
|
||||
/tools/toolchains/bin
|
||||
/tools/toolchains/rvtc-medany/bin
|
||||
)
|
||||
fi
|
||||
|
||||
|
||||
@@ -107,7 +107,12 @@ $(BOOTLOADER_IMG): FORCE $(BOOTLOADER_SW_SRCS)
|
||||
|
||||
$(APP_ELF): $(APP_SW_SRCS)
|
||||
test -d "$(APP_SW_DIR)"
|
||||
$(MAKE) -C "$(APP_SW_DIR)" TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(APP_BUILD_DIR)" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" $(SIM_APP_MAKE_ARGS) all
|
||||
$(MAKE) -C "$(APP_SW_DIR)" TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(APP_BUILD_DIR)" \
|
||||
GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" \
|
||||
BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" \
|
||||
BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" \
|
||||
BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" \
|
||||
$(SIM_APP_MAKE_ARGS) all
|
||||
|
||||
$(APP_FLASH_BIN): $(APP_ELF)
|
||||
mkdir -p "$(FLASH_DIR)"
|
||||
|
||||
@@ -24,7 +24,7 @@ GDB_HOST ?= localhost
|
||||
GDB_PORT ?= 3333
|
||||
OPENOCD_ADAPTER_SPEED ?= 1000
|
||||
UART_PORT ?= auto
|
||||
UART_BAUD ?= 115200
|
||||
UART_BAUD ?= 460800
|
||||
BOARD ?= genesys2
|
||||
PART_NAME ?= xc7k325tffg900-2
|
||||
BOARD_PART ?= digilentinc.com:genesys2:part0:1.1
|
||||
@@ -69,6 +69,8 @@ FLASH_REGIONS ?= auto
|
||||
FLASH_FORCE ?= 0
|
||||
GPS_DEBUG ?= 1
|
||||
GPS_DEBUG_LEVEL ?= 1
|
||||
GPS_ESKF_ENABLE ?= 1
|
||||
IMU_SENSOR ?= auto
|
||||
|
||||
FLASH_MCS ?= $(OUT_DIR)/flash/genesys2_rocket.mcs
|
||||
FLASH_BIN ?= $(OUT_DIR)/flash/genesys2_rocket.bin
|
||||
@@ -172,6 +174,8 @@ help:
|
||||
' FLASH_FORCE=$(FLASH_FORCE) # 1 forces unchanged regions too' \
|
||||
' APP=$(APP)' \
|
||||
' APP_ELF=$(APP_ELF)' \
|
||||
' GPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE)' \
|
||||
' IMU_SENSOR=$(IMU_SENSOR)' \
|
||||
' UART_PORT=$(UART_PORT)' \
|
||||
' ETH_RF_IFACE=$(ETH_RF_IFACE)' \
|
||||
' ETH_RF_INPUT=$(ETH_RF_INPUT)' \
|
||||
@@ -209,15 +213,17 @@ fpga-config:
|
||||
' CFGMEM_PART=$(CFGMEM_PART)' \
|
||||
' UART_PORT=$(UART_PORT)' \
|
||||
' UART_BAUD=$(UART_BAUD)' \
|
||||
' GPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE)' \
|
||||
' IMU_SENSOR=$(IMU_SENSOR)' \
|
||||
' ETH_RF_IFACE=$(ETH_RF_IFACE)' \
|
||||
' ETH_RF_INPUT=$(ETH_RF_INPUT)' \
|
||||
' ETH_RF_SAMPLE_FORMAT=$(ETH_RF_SAMPLE_FORMAT)'
|
||||
|
||||
software:
|
||||
@$(MAKE_BIN) -C "$(SOC_SW_DIR)" TOOLCHAIN="$(TOOLCHAIN)" APP="$(APP)" APP_FLASH_OFFSET="$(APP_FLASH_OFFSET)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(DTIM_BASE)" BSP_DTIM_SIZE="$(DTIM_SIZE)" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" bootloader flash-image
|
||||
@$(MAKE_BIN) -C "$(SOC_SW_DIR)" TOOLCHAIN="$(TOOLCHAIN)" APP="$(APP)" APP_FLASH_OFFSET="$(APP_FLASH_OFFSET)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(DTIM_BASE)" BSP_DTIM_SIZE="$(DTIM_SIZE)" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" GPS_ESKF_ENABLE="$(GPS_ESKF_ENABLE)" IMU_SENSOR="$(IMU_SENSOR)" bootloader flash-image
|
||||
|
||||
$(APP_ELF): FORCE
|
||||
@$(MAKE_BIN) -C "$(SOC_SW_DIR)" TOOLCHAIN="$(TOOLCHAIN)" APP="$(APP)" APP_FLASH_OFFSET="$(APP_FLASH_OFFSET)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(DTIM_BASE)" BSP_DTIM_SIZE="$(DTIM_SIZE)" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" app
|
||||
@$(MAKE_BIN) -C "$(SOC_SW_DIR)" TOOLCHAIN="$(TOOLCHAIN)" APP="$(APP)" APP_FLASH_OFFSET="$(APP_FLASH_OFFSET)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(DTIM_BASE)" BSP_DTIM_SIZE="$(DTIM_SIZE)" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" GPS_ESKF_ENABLE="$(GPS_ESKF_ENABLE)" IMU_SENSOR="$(IMU_SENSOR)" app
|
||||
|
||||
chisel: software
|
||||
@cd "$(REPO_DIR)"
|
||||
|
||||
@@ -16,7 +16,7 @@ Varsayilanlar:
|
||||
SoC/fabric clock : 50 MHz
|
||||
GPS acquisition : 80 MHz
|
||||
GPS tracking : 125 MHz
|
||||
UART baud : 115200
|
||||
UART baud : 460800
|
||||
App flash offset : 0x01800000
|
||||
QSPI flash : 32 MiB S25FL256S
|
||||
RF input mode : pmod
|
||||
@@ -181,7 +181,7 @@ make -C targets/fpga/genesys2 bitstream
|
||||
make -C targets/fpga/genesys2 program-fpga
|
||||
make -C targets/fpga/genesys2 program-flash
|
||||
make -C targets/fpga/genesys2 program-app APP=gps_system
|
||||
make -C targets/fpga/genesys2 uart-listen UART_PORT=/dev/ttyUSB1 UART_BAUD=115200
|
||||
make -C targets/fpga/genesys2 uart-listen UART_PORT=/dev/ttyUSB1
|
||||
```
|
||||
|
||||
Kok Makefile uzerinden:
|
||||
@@ -190,12 +190,25 @@ Kok Makefile uzerinden:
|
||||
make FPGA_BOARD=genesys2 fpga-config
|
||||
make FPGA_BOARD=genesys2 fpga-bitstream
|
||||
make FPGA_BOARD=genesys2 fpga-program
|
||||
|
||||
# TDK + GPS + ESKF, UART varsayilani 460800
|
||||
make FPGA_BOARD=genesys2 APP=gps_system \
|
||||
GPS_ESKF_ENABLE=1 IMU_SENSOR=tdk fpga-mcs
|
||||
|
||||
# GPS-only; tracking, Doppler velocity WLS ve PPS aktif
|
||||
make FPGA_BOARD=genesys2 APP=gps_system \
|
||||
GPS_ESKF_ENABLE=0 fpga-mcs
|
||||
```
|
||||
|
||||
`program-app APP=gps_system` yalniz application flash bolgesini derleyip
|
||||
programlar. Tracking mailbox registerlari ve synchronizer RTL degisikligi
|
||||
bitstream gerektirir; Doppler IIR katsayisi/yazilim degisikligi ise bitstream
|
||||
gerektirmez, yalniz yeni `gps_system` uygulamasinin yuklenmesi yeterlidir.
|
||||
`GPS_ESKF_ENABLE` verilmezse ESKF aciktir. ESKF kapali profilde IMU ve fusion
|
||||
baslatilmaz; GPS acquisition/tracking, C/N0, Doppler velocity WLS, ham konum ve
|
||||
PPS calismaya devam eder. Iki profil ayni `genesys2_rocket.mcs/.bin` cikti
|
||||
yoluna yazdigi icin birlikte saklanacaksa build sonrasinda yeniden
|
||||
adlandirilmalidir.
|
||||
|
||||
`debug-load APP=gps_system` ELF'i GDB ile DTIM/RAM'e yukler; hizli kart testi
|
||||
icin uygundur fakat power-cycle/reset sonrasinda flash'taki eski uygulama geri
|
||||
|
||||
@@ -0,0 +1,12 @@
|
||||
/out/
|
||||
/flow/efinity/ti375c529_rocket.isf
|
||||
/flow/efinity/ti375c529_rocket.peri.xml
|
||||
/flow/efinity/ti375c529_rocket.pt.sdc
|
||||
/flow/efinity/ti375c529_rocket.pt.rpt
|
||||
/flow/efinity/ti375c529_rocket.pinout.*
|
||||
/flow/efinity/ti375c529_rocket.interface.csv
|
||||
/flow/efinity/ti375c529_rocket_template.v
|
||||
/flow/efinity/outflow/
|
||||
/flow/efinity/work_pnr/
|
||||
/flow/efinity/work_syn/
|
||||
/flow/efinity/RUNPNR_*
|
||||
@@ -0,0 +1,408 @@
|
||||
SHELL := /bin/bash
|
||||
.SHELLFLAGS := -euo pipefail -c
|
||||
.ONESHELL:
|
||||
|
||||
FPGA_DIR := $(abspath $(dir $(lastword $(MAKEFILE_LIST))))
|
||||
REPO_DIR := $(abspath $(FPGA_DIR)/../../..)
|
||||
SOC_SW_DIR := $(REPO_DIR)/soc/software
|
||||
SOC_OUT_DIR := $(REPO_DIR)/soc/out
|
||||
HW_DIR := $(REPO_DIR)/hardware
|
||||
HW_GENERATED_DIR := $(HW_DIR)/generated
|
||||
GPS_SYSTEM_DIR ?= $(HW_DIR)/external/gps/rtl
|
||||
FLOW_DIR := $(FPGA_DIR)/flow/efinity
|
||||
DEBUG_DIR := $(FPGA_DIR)/flow/debug
|
||||
TOOL_SCRIPT_DIR := $(FPGA_DIR)/tools
|
||||
OUT_DIR := $(FPGA_DIR)/out
|
||||
LOG_DIR := $(OUT_DIR)/logs
|
||||
|
||||
include $(REPO_DIR)/tools/project.mk
|
||||
|
||||
BOARD ?= ti375c529
|
||||
DEVICE ?= Ti375C529
|
||||
TIMING_MODEL ?= C4
|
||||
SOC_CLK_HZ ?= 100000000
|
||||
GPS_ACQ_CLK_HZ ?= 100000000
|
||||
GPS_TRCK_CLK_HZ ?= 125000000
|
||||
GPS_RF_CLK_HZ ?= 16367981
|
||||
RF_INPUT_MODE ?= pmod
|
||||
UART_PORT ?= /dev/ttyUSB2
|
||||
UART_BAUD ?= 460800
|
||||
OPENOCD ?= $(shell command -v openocd)
|
||||
OPENOCD_ADAPTER_SPEED ?= 1000
|
||||
GDB_HOST ?= localhost
|
||||
GDB_PORT ?= 3333
|
||||
GDB ?= $(firstword $(wildcard /tools/toolchain/bin/riscv64-unknown-elf-gdb) $(CROSS)gdb)
|
||||
# Leave empty for Efinity's board-profile auto-detection. Boards without an
|
||||
# FTDI serial number require a bus/device-qualified URL, which changes after
|
||||
# re-enumeration; users with multiple boards can override this explicitly.
|
||||
FTDI_URL ?=
|
||||
BSP_UART_CLOCK_HZ ?= $(SOC_CLK_HZ)
|
||||
BSP_UART_BAUDRATE ?= $(UART_BAUD)
|
||||
BSP_CLINT_TIMEBASE_HZ ?= 1000000
|
||||
DTIM_BASE ?= 0x80000000
|
||||
DTIM_SIZE ?= 0x00100000
|
||||
# The compressed Ti375 configuration currently occupies about 5.2 MB.
|
||||
# Keep the application on a 64-KiB boundary with ample growth margin.
|
||||
APP_FLASH_OFFSET ?= 0x00800000
|
||||
FLASH_SIZE ?= 0x04000000
|
||||
GPS_DEBUG ?= 1
|
||||
GPS_DEBUG_LEVEL ?= 4
|
||||
GPS_ESKF_ENABLE ?= 1
|
||||
IMU_SENSOR ?= auto
|
||||
app ?=
|
||||
ifneq ($(strip $(app)),)
|
||||
APP ?= $(app)
|
||||
else
|
||||
APP ?= hello_uart
|
||||
endif
|
||||
|
||||
FIRRTL_DIR ?= $(HW_GENERATED_DIR)/ti375c529-soc
|
||||
VERILOG_DIR ?= $(HW_GENERATED_DIR)/ti375c529-soc-verilog
|
||||
WRAPPER := $(OUT_DIR)/build/generated/ti375c529_rocket_top.sv
|
||||
GPS_EFINITY_DIR := $(OUT_DIR)/build/generated/gps_efinity
|
||||
SOURCE_LIST := $(OUT_DIR)/build/efinity_sources.f
|
||||
PROJECT_XML := $(FLOW_DIR)/ti375c529_rocket.xml
|
||||
INTERFACE_SOURCE := $(HW_DIR)/fpga/ti375c529/constraints/ti375c529_rocket.isf
|
||||
INTERFACE_SCRIPT := $(FLOW_DIR)/ti375c529_rocket.isf
|
||||
BIT_FILE ?= $(OUT_DIR)/bitstream/ti375c529_rocket.bit
|
||||
CONFIG_HEX ?= $(OUT_DIR)/bitstream/ti375c529_rocket.hex
|
||||
APP_ELF ?= $(SOC_OUT_DIR)/software/$(APP)/$(APP).elf
|
||||
APP_FLASH_IMAGE ?= $(SOC_OUT_DIR)/flash/$(APP).flash.bin
|
||||
FLASH_DIR := $(OUT_DIR)/flash
|
||||
FLASH_REGION_DIR := $(FLASH_DIR)/regions
|
||||
APP_HEX ?= $(FLASH_REGION_DIR)/app.hex
|
||||
FLASH_HEX ?= $(FLASH_DIR)/ti375c529_rocket.hex
|
||||
FLASH_MANIFEST ?= $(FLASH_DIR)/ti375c529_rocket.json
|
||||
FLASH_LOADER ?= $(EFINITY_HOME)/pgm/fli/titanium/u006A0A79.bit
|
||||
FLASH_VERIFY ?= onchipx2
|
||||
OPENOCD_CFG ?= $(DEBUG_DIR)/openocd_ti375c529_bscan.cfg
|
||||
GDB_LOAD_SCRIPT ?= $(DEBUG_DIR)/gdb_load_and_run.gdb
|
||||
GDB_ATTACH_SCRIPT ?= $(DEBUG_DIR)/gdb_attach.gdb
|
||||
|
||||
.PHONY: help all fpga-config detect software chisel rtl soc interface map bitstream \
|
||||
program-fpga debug-openocd debug-load debug-gdb uart-listen \
|
||||
flash-image flash-regions program-flash program-flash-elf \
|
||||
program-flash-bitstream program-flash-regions program-flash-all \
|
||||
program-flash-combined program-flash-loader program-app program-bitstream \
|
||||
clean clean-sw clean-fpga FORCE
|
||||
|
||||
help:
|
||||
@printf '%s\n' \
|
||||
'Ti375C529 Efinity targets:' \
|
||||
' make detect Scan USB and read the Ti375 JTAG IDCODE' \
|
||||
' make software Build the Rocket boot ROM and selected RV64 app' \
|
||||
' make chisel Emit Rocket FIRRTL with the existing FPGA config' \
|
||||
' make rtl Emit split SystemVerilog, Efinix shell, and file list' \
|
||||
' make interface Generate Efinity pin/PLL/JTAG interface data' \
|
||||
' make map Run Efinity synthesis only' \
|
||||
' make bitstream Run the complete Efinity flow' \
|
||||
' make program-fpga Load BIT_FILE into Ti375 SRAM over JTAG' \
|
||||
' make flash-image Generate combined Lower-flash Efinity HEX' \
|
||||
' make program-flash Program only the application flash region' \
|
||||
' make program-flash-bitstream' \
|
||||
' Program only the configuration region' \
|
||||
' make program-flash-combined' \
|
||||
' Program configuration + application HEX' \
|
||||
' make debug-openocd Start Rocket DMI/GDB server through JTAG_USER1' \
|
||||
' make debug-load Load APP ELF into DTIM through GDB' \
|
||||
' make uart-listen Listen to FTDI UART interface 2' \
|
||||
' make fpga-config Print resolved tools, clocks, files, and USB config'
|
||||
|
||||
all: bitstream
|
||||
|
||||
fpga-config:
|
||||
@printf '%s\n' \
|
||||
'Ti375C529 FPGA config:' \
|
||||
' EFINITY_HOME=$(EFINITY_HOME)' \
|
||||
' EFX_RUN=$(EFX_RUN)' \
|
||||
' EFX_PGM=$(EFX_PGM)' \
|
||||
' OPENOCD=$(OPENOCD)' \
|
||||
' GDB=$(GDB)' \
|
||||
' DEVICE=$(DEVICE)' \
|
||||
' TIMING_MODEL=$(TIMING_MODEL)' \
|
||||
' SOC_CLK_HZ=$(SOC_CLK_HZ)' \
|
||||
' GPS_ACQ_CLK_HZ=$(GPS_ACQ_CLK_HZ)' \
|
||||
' GPS_TRCK_CLK_HZ=$(GPS_TRCK_CLK_HZ)' \
|
||||
' GPS_RF_CLK_HZ=$(GPS_RF_CLK_HZ)' \
|
||||
' RF_INPUT_MODE=$(RF_INPUT_MODE)' \
|
||||
' BIT_FILE=$(BIT_FILE)' \
|
||||
' CONFIG_HEX=$(CONFIG_HEX)' \
|
||||
' FLASH_HEX=$(FLASH_HEX)' \
|
||||
' APP_FLASH_OFFSET=$(APP_FLASH_OFFSET)' \
|
||||
' FLASH_LOADER=$(FLASH_LOADER)' \
|
||||
' FTDI_URL=$(FTDI_URL)' \
|
||||
' UART_PORT=$(UART_PORT)' \
|
||||
' UART_BAUD=$(UART_BAUD)'
|
||||
|
||||
detect:
|
||||
@set +u
|
||||
source "$(EFINITY_SETUP)" >/dev/null 2>&1
|
||||
set -u
|
||||
"$(EFX_PGM)" --scan_usb
|
||||
|
||||
software:
|
||||
@$(MAKE) -C "$(SOC_SW_DIR)" \
|
||||
TOOLCHAIN="$(TOOLCHAIN)" APP="$(APP)" \
|
||||
APP_FLASH_OFFSET="$(APP_FLASH_OFFSET)" \
|
||||
BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" \
|
||||
BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" \
|
||||
GPS_SYSTEM_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" \
|
||||
BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" \
|
||||
BSP_DTIM_BASE="$(DTIM_BASE)" BSP_DTIM_SIZE="$(DTIM_SIZE)" \
|
||||
GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" \
|
||||
GPS_ESKF_ENABLE="$(GPS_ESKF_ENABLE)" IMU_SENSOR="$(IMU_SENSOR)" \
|
||||
bootloader flash-image
|
||||
|
||||
$(APP_ELF): FORCE
|
||||
@$(MAKE) -C "$(SOC_SW_DIR)" \
|
||||
TOOLCHAIN="$(TOOLCHAIN)" APP="$(APP)" \
|
||||
BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" \
|
||||
BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" \
|
||||
GPS_SYSTEM_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" \
|
||||
BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" \
|
||||
BSP_DTIM_BASE="$(DTIM_BASE)" BSP_DTIM_SIZE="$(DTIM_SIZE)" \
|
||||
GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" \
|
||||
GPS_ESKF_ENABLE="$(GPS_ESKF_ENABLE)" IMU_SENSOR="$(IMU_SENSOR)" app
|
||||
|
||||
chisel: software
|
||||
@cd "$(REPO_DIR)"
|
||||
"$(MILL)" -i 'rocketchip[6.7.0].assembly'
|
||||
rm -rf "$(FIRRTL_DIR)"
|
||||
mkdir -p "$(FIRRTL_DIR)"
|
||||
"$(JAVA_HOME)/bin/java" \
|
||||
-DROCKET_DTIM_BASE="$(DTIM_BASE)" \
|
||||
-DROCKET_DTIM_SIZE="$(DTIM_SIZE)" \
|
||||
-jar out/rocketchip/6.7.0/assembly.dest/out.jar \
|
||||
--dir "$(FIRRTL_DIR)" \
|
||||
--top freechips.rocketchip.fpga.VCU118RocketSystem \
|
||||
--config freechips.rocketchip.fpga.VCU118RocketConfig
|
||||
|
||||
rtl: chisel
|
||||
@cd "$(REPO_DIR)"
|
||||
rm -rf "$(VERILOG_DIR)"
|
||||
mkdir -p "$(VERILOG_DIR)" "$(dir $(WRAPPER))"
|
||||
$(FIRTOOL_ENV) "$(FIRTOOL)" \
|
||||
"$(FIRRTL_DIR)/VCU118RocketSystem.fir" \
|
||||
--split-verilog -o "$(VERILOG_DIR)" \
|
||||
--preserve-aggregate=none \
|
||||
--disable-annotation-unknown \
|
||||
--disable-all-randomization \
|
||||
--strip-debug-info
|
||||
find "$(VERILOG_DIR)" -maxdepth 1 -type f \( -name '*.sv' -o -name '*.v' \) \
|
||||
-printf './%f\n' | sort > "$(VERILOG_DIR)/filelist.f"
|
||||
perl -0pi -e 's/^([ \t]*)if \([^\n]*\)\n[ \t]*assert__[^\n]*:\s*assert\([^\n]*\);\n//mg; s/^[ \t]*assert__[^\n]*:\s*assert\([^\n]*\);\n//mg' "$(VERILOG_DIR)"/*.sv
|
||||
python3 "$(TOOL_SCRIPT_DIR)/prepare_efinity_rtl.py" \
|
||||
--verilog-dir "$(VERILOG_DIR)" \
|
||||
--gps-source-dir "$(GPS_SYSTEM_DIR)" \
|
||||
--gps-output-dir "$(GPS_EFINITY_DIR)"
|
||||
python3 "$(TOOL_SCRIPT_DIR)/gen_ti375c529_wrapper.py" \
|
||||
--rocket-top "$(VERILOG_DIR)/VCU118RocketSystem.sv" \
|
||||
--out "$(WRAPPER)"
|
||||
: > "$(SOURCE_LIST)"
|
||||
while IFS= read -r rel; do
|
||||
rel="$${rel#./}"
|
||||
printf '%s\n' "$(VERILOG_DIR)/$$rel" >> "$(SOURCE_LIST)"
|
||||
done < "$(VERILOG_DIR)/filelist.f"
|
||||
printf '%s\n' "$(HW_DIR)/upstream/rocket-chip/src/main/resources/vsrc/plusarg_reader.v" >> "$(SOURCE_LIST)"
|
||||
while IFS= read -r rel; do
|
||||
[[ -z "$$rel" || "$$rel" == \#* ]] && continue
|
||||
case "$$rel" in
|
||||
trck_src/xil_fifo.sv|trck_src/pulp_fifo/top.sv) continue ;;
|
||||
acq_src/acq_nco_carr.sv)
|
||||
printf '%s\n' "$(GPS_EFINITY_DIR)/acq_nco_carr.sv" >> "$(SOURCE_LIST)"
|
||||
continue ;;
|
||||
acq_src/twiddle_ram.sv)
|
||||
printf '%s\n' "$(GPS_EFINITY_DIR)/twiddle_ram.sv" >> "$(SOURCE_LIST)"
|
||||
continue ;;
|
||||
trck_src/NCO_carr.sv)
|
||||
printf '%s\n' "$(GPS_EFINITY_DIR)/NCO_carr.sv" >> "$(SOURCE_LIST)"
|
||||
continue ;;
|
||||
esac
|
||||
printf '%s\n' "$(GPS_SYSTEM_DIR)/$$rel" >> "$(SOURCE_LIST)"
|
||||
done < "$(GPS_SYSTEM_DIR)/filelist.f"
|
||||
# Efinix JTAG_USER1 has the same two-bit TDO lead-in as the Kintex-7
|
||||
# BSCANE2 path, so use the Genesys2-aligned Rocket DMI tunnel.
|
||||
printf '%s\n' "$(HW_DIR)/fpga/genesys2/rtl/rocket_bscan_dmi_tunnel.sv" >> "$(SOURCE_LIST)"
|
||||
$(MAKE) "$(INTERFACE_SCRIPT)"
|
||||
|
||||
soc: rtl
|
||||
|
||||
$(INTERFACE_SCRIPT): $(INTERFACE_SOURCE)
|
||||
cp "$<" "$@"
|
||||
|
||||
interface: $(INTERFACE_SCRIPT)
|
||||
@mkdir -p "$(LOG_DIR)"
|
||||
cd "$(FLOW_DIR)"
|
||||
# Efinity prefers a matching generated .peri.xml over the source .isf.
|
||||
# Remove it so pin/PLL changes in the checked-in interface script are applied.
|
||||
rm -f "$(FLOW_DIR)/ti375c529_rocket.peri.xml"
|
||||
set +u
|
||||
source "$(EFINITY_SETUP)" >/dev/null 2>&1
|
||||
set -u
|
||||
python3 "$(EFX_RUN)" "$(PROJECT_XML)" --flow interface --un_flow \
|
||||
--pt_opts "peri_scripts=$(INTERFACE_SCRIPT)" 2>&1 | tee "$(LOG_DIR)/interface.log"
|
||||
|
||||
map: rtl interface
|
||||
@mkdir -p "$(LOG_DIR)"
|
||||
cd "$(FLOW_DIR)"
|
||||
set +u
|
||||
source "$(EFINITY_SETUP)" >/dev/null 2>&1
|
||||
set -u
|
||||
python3 "$(EFX_RUN)" "$(PROJECT_XML)" --flow map 2>&1 | tee "$(LOG_DIR)/map.log"
|
||||
|
||||
bitstream: rtl interface
|
||||
@mkdir -p "$(LOG_DIR)" "$(dir $(BIT_FILE))"
|
||||
cd "$(FLOW_DIR)"
|
||||
set +u
|
||||
source "$(EFINITY_SETUP)" >/dev/null 2>&1
|
||||
set -u
|
||||
: > "$(LOG_DIR)/full.log"
|
||||
python3 "$(EFX_RUN)" "$(PROJECT_XML)" --flow map 2>&1 | tee -a "$(LOG_DIR)/full.log"
|
||||
python3 "$(EFX_RUN)" "$(PROJECT_XML)" --flow pnr --work_dir work_pnr 2>&1 | tee -a "$(LOG_DIR)/full.log"
|
||||
python3 "$(EFX_RUN)" "$(PROJECT_XML)" --flow pgm --work_dir work_pnr 2>&1 | tee -a "$(LOG_DIR)/full.log"
|
||||
generated_bit="$$(find "$(FLOW_DIR)/outflow" "$(OUT_DIR)" -type f -name 'ti375c529_rocket.bit' 2>/dev/null | head -n 1)"
|
||||
if [[ -z "$$generated_bit" ]]; then
|
||||
echo "Efinity completed without a discoverable ti375c529_rocket.bit" >&2
|
||||
exit 1
|
||||
fi
|
||||
if [[ "$$generated_bit" != "$(BIT_FILE)" ]]; then
|
||||
cp "$$generated_bit" "$(BIT_FILE)"
|
||||
fi
|
||||
generated_hex="$$(find "$(FLOW_DIR)/outflow" "$(OUT_DIR)" -type f -name 'ti375c529_rocket.hex' ! -path '$(FLASH_DIR)/*' 2>/dev/null | head -n 1)"
|
||||
if [[ -z "$$generated_hex" ]]; then
|
||||
echo "Efinity completed without a discoverable ti375c529_rocket.hex" >&2
|
||||
exit 1
|
||||
fi
|
||||
if [[ "$$generated_hex" != "$(CONFIG_HEX)" ]]; then
|
||||
cp "$$generated_hex" "$(CONFIG_HEX)"
|
||||
fi
|
||||
printf 'Bitstream: %s\nConfiguration HEX: %s\n' "$(BIT_FILE)" "$(CONFIG_HEX)"
|
||||
|
||||
program-fpga:
|
||||
@if [[ ! -f "$(BIT_FILE)" ]]; then
|
||||
echo "Missing bitstream: $(BIT_FILE)" >&2
|
||||
echo "Run: make -C $(FPGA_DIR) bitstream" >&2
|
||||
exit 1
|
||||
fi
|
||||
set +u
|
||||
source "$(EFINITY_SETUP)" >/dev/null 2>&1
|
||||
set -u
|
||||
ftdi_args=()
|
||||
if [[ -n "$(FTDI_URL)" ]]; then
|
||||
ftdi_args+=(--url "$(FTDI_URL)")
|
||||
fi
|
||||
"$(EFX_PGM)" --mode jtag "$${ftdi_args[@]}" "$(BIT_FILE)"
|
||||
|
||||
debug-openocd:
|
||||
@"$(OPENOCD)" \
|
||||
-c "set ADAPTER_SPEED $(OPENOCD_ADAPTER_SPEED)" \
|
||||
-c "gdb_port $(GDB_PORT)" \
|
||||
-f "$(OPENOCD_CFG)"
|
||||
|
||||
debug-load: $(APP_ELF)
|
||||
@"$(GDB)" -q "$(APP_ELF)" \
|
||||
-ex "set confirm off" \
|
||||
-ex "set pagination off" \
|
||||
-ex "target extended-remote $(GDB_HOST):$(GDB_PORT)" \
|
||||
-x "$(GDB_LOAD_SCRIPT)"
|
||||
|
||||
debug-gdb: $(APP_ELF)
|
||||
@"$(GDB)" -q "$(APP_ELF)" \
|
||||
-ex "set confirm off" \
|
||||
-ex "set pagination off" \
|
||||
-ex "target extended-remote $(GDB_HOST):$(GDB_PORT)" \
|
||||
-x "$(GDB_ATTACH_SCRIPT)"
|
||||
|
||||
uart-listen:
|
||||
@stty -F "$(UART_PORT)" "$(UART_BAUD)" cs8 -cstopb -parenb -ixon -ixoff raw
|
||||
@cat "$(UART_PORT)"
|
||||
|
||||
flash-image: flash-regions
|
||||
|
||||
flash-regions: software
|
||||
@if [[ ! -f "$(CONFIG_HEX)" ]]; then
|
||||
echo "Missing Efinity configuration HEX: $(CONFIG_HEX)" >&2
|
||||
echo "Run: make -C $(FPGA_DIR) bitstream" >&2
|
||||
exit 1
|
||||
fi
|
||||
mkdir -p "$(FLASH_DIR)" "$(FLASH_REGION_DIR)"
|
||||
python3 "$(TOOL_SCRIPT_DIR)/make_flash_hex.py" \
|
||||
--config-hex "$(CONFIG_HEX)" \
|
||||
--app-bin "$(APP_FLASH_IMAGE)" \
|
||||
--app-offset "$(APP_FLASH_OFFSET)" \
|
||||
--flash-size "$(FLASH_SIZE)" \
|
||||
--combined-hex "$(FLASH_HEX)" \
|
||||
--app-hex "$(APP_HEX)" \
|
||||
--manifest "$(FLASH_MANIFEST)"
|
||||
|
||||
program-flash-loader:
|
||||
@if [[ ! -f "$(FLASH_LOADER)" ]]; then
|
||||
echo "Missing Ti375 JTAG-to-SPI loader: $(FLASH_LOADER)" >&2
|
||||
exit 1
|
||||
fi
|
||||
set +u
|
||||
source "$(EFINITY_SETUP)" >/dev/null 2>&1
|
||||
set -u
|
||||
ftdi_args=()
|
||||
if [[ -n "$(FTDI_URL)" ]]; then
|
||||
ftdi_args+=(--url "$(FTDI_URL)")
|
||||
fi
|
||||
"$(EFX_PGM)" --mode jtag "$${ftdi_args[@]}" "$(FLASH_LOADER)"
|
||||
|
||||
program-flash program-flash-elf program-app: flash-regions
|
||||
@$(MAKE) -C "$(FPGA_DIR)" program-flash-loader
|
||||
set +u
|
||||
source "$(EFINITY_SETUP)" >/dev/null 2>&1
|
||||
set -u
|
||||
ftdi_args=()
|
||||
if [[ -n "$(FTDI_URL)" ]]; then
|
||||
ftdi_args+=(--url "$(FTDI_URL)")
|
||||
fi
|
||||
"$(EFX_PGM)" --mode jtag_bridge --jtag_bridge_mode all \
|
||||
--verify_method "$(FLASH_VERIFY)" --address "$(APP_FLASH_OFFSET)" \
|
||||
"$${ftdi_args[@]}" "$(APP_HEX)"
|
||||
|
||||
program-flash-bitstream program-bitstream:
|
||||
@if [[ ! -f "$(CONFIG_HEX)" ]]; then
|
||||
echo "Missing Efinity configuration HEX: $(CONFIG_HEX)" >&2
|
||||
echo "Run: make -C $(FPGA_DIR) bitstream" >&2
|
||||
exit 1
|
||||
fi
|
||||
$(MAKE) -C "$(FPGA_DIR)" program-flash-loader
|
||||
set +u
|
||||
source "$(EFINITY_SETUP)" >/dev/null 2>&1
|
||||
set -u
|
||||
ftdi_args=()
|
||||
if [[ -n "$(FTDI_URL)" ]]; then
|
||||
ftdi_args+=(--url "$(FTDI_URL)")
|
||||
fi
|
||||
"$(EFX_PGM)" --mode jtag_bridge --jtag_bridge_mode all \
|
||||
--verify_method "$(FLASH_VERIFY)" --address 0 \
|
||||
"$${ftdi_args[@]}" "$(CONFIG_HEX)"
|
||||
|
||||
program-flash-regions program-flash-all program-flash-combined: flash-image
|
||||
@$(MAKE) -C "$(FPGA_DIR)" program-flash-loader
|
||||
set +u
|
||||
source "$(EFINITY_SETUP)" >/dev/null 2>&1
|
||||
set -u
|
||||
ftdi_args=()
|
||||
if [[ -n "$(FTDI_URL)" ]]; then
|
||||
ftdi_args+=(--url "$(FTDI_URL)")
|
||||
fi
|
||||
"$(EFX_PGM)" --mode jtag_bridge --jtag_bridge_mode all \
|
||||
--verify_method "$(FLASH_VERIFY)" --address 0 \
|
||||
"$${ftdi_args[@]}" "$(FLASH_HEX)"
|
||||
|
||||
clean-sw:
|
||||
@$(MAKE) -C "$(SOC_SW_DIR)" clean
|
||||
|
||||
clean-fpga:
|
||||
rm -rf "$(OUT_DIR)" "$(FIRRTL_DIR)" "$(VERILOG_DIR)"
|
||||
rm -f "$(INTERFACE_SCRIPT)" \
|
||||
"$(FLOW_DIR)/ti375c529_rocket.peri.xml" \
|
||||
"$(FLOW_DIR)/ti375c529_rocket.pt.sdc" \
|
||||
"$(FLOW_DIR)/ti375c529_rocket.pt.rpt"
|
||||
|
||||
clean: clean-fpga
|
||||
|
||||
FORCE:
|
||||
@@ -0,0 +1,160 @@
|
||||
# Ti375C529 Rocket FPGA flow
|
||||
|
||||
This target keeps the project's RV64 Rocket/GPS subsystem in FPGA fabric and
|
||||
replaces only the Xilinx board shell and build flow with Efinix equivalents.
|
||||
It does not use the Ti375 hardened RV32 subsystem.
|
||||
|
||||
Detailed porting, synthesis, clocking, JTAG, and verification notes are in
|
||||
[`flow/efinity/README.md`](flow/efinity/README.md).
|
||||
|
||||
## Build and program
|
||||
|
||||
```bash
|
||||
make FPGA_BOARD=ti375c529 fpga-config
|
||||
make FPGA_BOARD=ti375c529 fpga-rtl
|
||||
make FPGA_BOARD=ti375c529 fpga-bitstream
|
||||
make FPGA_BOARD=ti375c529 fpga-program
|
||||
```
|
||||
|
||||
The Makefile auto-discovers Efinity under `/tools/efinix/efinity`. Override
|
||||
`EFINITY_HOME` when another installation is required.
|
||||
|
||||
Programming auto-selects the connected Ti375 development kit. If more than
|
||||
one compatible FTDI device is attached, run `make FPGA_BOARD=ti375c529
|
||||
fpga-detect` to list the bus-qualified URLs and pass the selected one as
|
||||
`FTDI_URL=...`.
|
||||
|
||||
Volatile JTAG programming and GDB loading into Rocket DTIM remain available.
|
||||
Persistent boot uses the board's Lower 512-Mbit SPI flash with this map:
|
||||
|
||||
| Region | Flash offset | Contents |
|
||||
|---|---:|---|
|
||||
| FPGA configuration | `0x00000000` | Efinity active-mode configuration HEX |
|
||||
| Rocket application | `0x00800000` | Boot header and ELF-derived payload |
|
||||
|
||||
The application starts at 8 MiB because the compressed Ti375 configuration is
|
||||
currently about 5.19 MB; the previous 4 MiB trial offset would overlap it.
|
||||
Generate or program the Efinity raw HEX images with:
|
||||
|
||||
```bash
|
||||
make FPGA_BOARD=ti375c529 fpga-mcs APP=hello_uart
|
||||
make FPGA_BOARD=ti375c529 fpga-flash-app APP=hello_uart
|
||||
make FPGA_BOARD=ti375c529 fpga-flash-bitstream
|
||||
make FPGA_BOARD=ti375c529 fpga-flash-mcs APP=hello_uart
|
||||
```
|
||||
|
||||
`fpga-mcs` is the repository-wide compatibility target name. For this Efinix
|
||||
board it produces `.hex`, not Xilinx `.mcs`. `fpga-flash-app` changes only the
|
||||
application region; `fpga-flash-mcs` erases/programs configuration plus the
|
||||
application through Efinity's pre-built JTAG-to-SPI bridge and verifies the
|
||||
readback. Cold boot from this layout was verified by a complete board power
|
||||
cycle: Ti375 configured from flash and Rocket loaded and completed
|
||||
`hello_uart` from the application region without a JTAG/GDB load.
|
||||
|
||||
## Debug and load
|
||||
|
||||
Use the host OpenOCD build, not the OpenOCD shipped with Efinity. The build
|
||||
must include `riscv set_bscan_tunnel_ir`; Ti375 uses a 5-bit outer TAP and
|
||||
selects `JTAG_USER1` with instruction `0x08`.
|
||||
OpenOCD 0.12 can be given that upstream-compatible command with
|
||||
`flow/debug/openocd-0.12-ti375-bscan.patch`.
|
||||
|
||||
In one terminal:
|
||||
|
||||
```bash
|
||||
make FPGA_BOARD=ti375c529 fpga-debug-openocd
|
||||
```
|
||||
|
||||
In a second terminal:
|
||||
|
||||
```bash
|
||||
make FPGA_BOARD=ti375c529 fpga-debug-load APP=hello_uart
|
||||
make FPGA_BOARD=ti375c529 fpga-uart
|
||||
```
|
||||
|
||||
The target deliberately selects `/tools/toolchain/bin/riscv64-unknown-elf-gdb`
|
||||
when it exists. The medany compiler toolchain is still used to build software,
|
||||
but it is not expected to contain GDB.
|
||||
|
||||
Efinity 2025.2 leaves Rocket's block-local packed store-data array undriven
|
||||
(`VDB-1002`). `tools/prepare_efinity_rtl.py` rewrites that generated RTL
|
||||
expression as an equivalent explicit `case`. Without the rewrite, DMI itself
|
||||
works, but GPR/CSR values stored into Debug Module data registers read as zero;
|
||||
OpenOCD consequently reports a misleading `misa=0`.
|
||||
|
||||
The patch is deliberately post-generation: Chisel and `firtool` first emit the
|
||||
Ti375 SystemVerilog tree, then `prepare_efinity_rtl.py` modifies only the
|
||||
generated Ti375 `Rocket.sv` before Efinity synthesis. It does not edit Rocket's
|
||||
native/upstream Chisel sources.
|
||||
|
||||
Two equivalent four-entry packed temporaries in the generated pbus/cbus
|
||||
`TLAtomicAutomata` modules are rewritten for the same reason, preserving the
|
||||
four atomic-operation mappings while removing their `VDB-1002` risk.
|
||||
|
||||
The same script generates Ti375-only copies of the acquisition carrier NCO,
|
||||
tracking carrier NCO, and FFT twiddle ROM under
|
||||
`out/build/generated/gps_efinity/`. This avoids Efinity 2025.2
|
||||
whole-array/`initial`/`$sin` elaboration failures without editing the shared GPS
|
||||
RTL. Only the Ti375 source list selects these copies; Genesys2 and VCU118 still
|
||||
use the originals. Detailed source paths, warning interpretation, and Questa
|
||||
RTL/post-map equivalence results are in the Efinity flow notes.
|
||||
|
||||
The affected generated source is
|
||||
`hardware/generated/ti375c529-soc-verilog/Rocket.sv`; Efinity records the
|
||||
original diagnostic in
|
||||
`targets/fpga/ti375c529/flow/efinity/outflow/ti375c529_rocket.warn.log`.
|
||||
The checked-in repair is
|
||||
`targets/fpga/ti375c529/tools/prepare_efinity_rtl.py` and runs after every
|
||||
`firtool` invocation, before Efinity synthesis.
|
||||
|
||||
## Board connections
|
||||
|
||||
PMOD0/J13 carries the GPS RF sampler:
|
||||
|
||||
| Signal | PMOD pin | FPGA resource | Package pin |
|
||||
|---|---:|---|---|
|
||||
| `rf_clk` | 10 | GPIOR_95_CLK12 | A12 |
|
||||
| `rf_data[0]` | 2 | GPIOR_89 | G16 |
|
||||
| `rf_data[1]` | 3 | GPIOR_90 | F16 |
|
||||
| `rf_data[2]` | 4 | GPIOR_91 | F17 |
|
||||
| `rf_data[3]` | 7 | GPIOR_92 | G17 |
|
||||
|
||||
`rf_clk` must be wired to J13 pin 10, not J13 pin 1. Pin 10 is the PMOD0
|
||||
header's dedicated 3.3 V global-clock input. J13 pin 1 (`G15`,
|
||||
`GPIOR_88_PLLIN1`) can feed a PLL but is not a dedicated fabric GCLK input;
|
||||
using it as the raw AFE clock makes Efinity promote normal GPIO routing into a
|
||||
GBUF and issue an extra-clock-insertion-delay warning.
|
||||
|
||||
As on Genesys2 and VCU118, the AX2771 clock is both the raw AFE sample clock
|
||||
and the source of the GPS processing-clock chain. Two Titanium PLLs generate
|
||||
an intermediate 55.5556 MHz clock, then 100 MHz acquisition and 125 MHz
|
||||
tracking clocks. The Rocket fabric uses the independent on-board 100 MHz
|
||||
oscillator.
|
||||
|
||||
PMOD1/J14 carries the sensor SPI, interrupt, and PPS:
|
||||
|
||||
| Signal | PMOD pin | FPGA resource | Package pin |
|
||||
|---|---:|---|---|
|
||||
| `pmod_spi_csn` | 1 | GPIOR_96_CLK13 | B12 |
|
||||
| `pmod_spi_dq[0]` | 2 | GPIOR_97 | C14 |
|
||||
| `pmod_spi_dq[1]` | 3 | GPIOR_98 | C13 |
|
||||
| `pmod_spi_sck` | 4 | GPIOR_99 | C12 |
|
||||
| `pmod_spi_dq[2]` | 7 | GPIOR_100 | D12 |
|
||||
| `pmod_spi_dq[3]` | 8 | GPIOR_101_PLLIN1 | F12 |
|
||||
| `pmod_irq` | 9 | GPIOR_102 | D13 |
|
||||
| `gps_pps` | 10 | GPIOR_103 | E13 |
|
||||
|
||||
The FTDI UART is `/dev/ttyUSB2` on the current Linux enumeration. SW3 is the
|
||||
active-low Rocket reset. JTAG uses Efinix `JTAG_USER1` and the existing Rocket
|
||||
DMI tunnel protocol.
|
||||
|
||||
The Lower flash user-mode connection is internal to the development kit:
|
||||
|
||||
| Signal | FPGA resource | Package pin |
|
||||
|---|---|---|
|
||||
| `flash_csn` | GPIOB_N_38 / SSL_N | A7 |
|
||||
| `flash_sck` | GPIOB_P_39 / CCK | G6 |
|
||||
| `flash_dq[0]` | GPIOB_N_40 / CDI0 | H4 |
|
||||
| `flash_dq[1]` | GPIOB_P_40 / CDI1 | H5 |
|
||||
| `flash_dq[2]` | GPIOB_N_39 / CDI2 | G5 |
|
||||
| `flash_dq[3]` | GPIOB_P_38 / CDI3 | B7 |
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user