diff --git a/soc/software/fusion/README.md b/soc/software/fusion/README.md index 1a10403..1e62cac 100644 --- a/soc/software/fusion/README.md +++ b/soc/software/fusion/README.md @@ -69,6 +69,435 @@ ESKF'nin anlamlı olması için GPS ve ham/decimate edilmiş IMU örnekleri ayn firmware çalışmasında ortak donanım zaman damgasıyla kaydedilmelidir; 500 ms ortalamalar hızlı dinamiği korumaz. +## Bare-metal C ESKF ve Linux doğrulaması (2026-07-13) + +> **Doğrulama durumu:** Bu ESKF/TDK entegrasyonu henüz real hardware üzerinde +> çalıştırılmadı. Kart test sırasında bağlı değildi. Aşağıdaki hareket, +> stationary, accuracy, outlier-rejection ve performans sonuçları Linux/GCC +> ortamında üretim bare-metal C kaynaklarıyla yapılan sentetik testlere aittir. +> Bunlar algoritma/regression doğrulamasıdır; gerçek sensör eksenleri, SPI/INT1, +> IRQ/sample kaybı, 50 MHz Rocket deadline'ı veya gerçek saha GPS doğruluğu için +> donanım kanıtı değildir. + +Python modelinden bağımsız olarak aynı algoritma artık `gps_system` içinde +freestanding C ile çalışır. Algoritma katmanı Linux testinde de kartta da aynı +kaynaklardan derlenir; Linux için ayrı veya sadeleştirilmiş bir ESKF kopyası +yoktur. + +Temel parçalar: + +- `gps_system/src/eskf.c`: ENU konum/hız, body-to-ENU quaternion, accel bias, + gyro bias ve 15x15 covariance içeren 15-hata-durumlu ESKF; +- `gps_system/src/fusion_motion.c`: bootstrap/stationary/moving/candidate + durum makinesi, filtrelenmiş jerk ve ZUPT izni; +- `gps_system/src/fusion_engine.c`: başlangıç kalibrasyonu, asenkron IMU/GPS, + NIS kapısı ve gecikmiş GPS için fixed-lag IMU replay; +- `gps_system/src/fusion_gps_adapter.c`: WGS-84 LLH/ECEF/ENU dönüşümü ve + covariance tabanı; +- `gps_system/src/tdk_icm42688.c`: ICM-42688-P SPI/INT1 sürücüsü; +- `gps_system/src/navigation_fusion.c`: GPS ve IMU kuyruklarını algoritmaya + bağlayan üst koordinatör. + +### ESKF state ve matematik sınırı + +Nominal state aşağıdaki bileşenlerden oluşur: + +```text +x = [p_ENU(3), v_ENU(3), q_body_to_ENU(4), b_accel(3), b_gyro(3)] +``` + +Covariance içinde quaternion'ın dört elemanı doğrudan tutulmaz. Sağ-taraflı +küçük açı hatası kullanılarak hata state'i 15 boyutta kalır: + +```text +delta_x = [delta_p(3), delta_v(3), delta_theta(3), + delta_b_accel(3), delta_b_gyro(3)] +``` + +IMU propagation'da ölçülen specific force ve angular rate'ten bias çıkarılır. +Quaternion body frame'den ENU frame'e dönüşümü temsil eder. ENU lineer ivmesi +`R(q) * (accel - b_accel) - [0, 0, g]` ile, quaternion ise gyro exponential +map ile ilerletilir. Büyük `dt` değerleri en fazla 20 ms'lik alt adımlara +bölünür. Covariance birinci dereceden error-state transition ve IMU/bias +process noise ile ilerletilir; her işlemden sonra simetrikleştirilir ve +diagonal tabanı uygulanır. + +GPS konum residual'ı `r = z_ENU - p_ENU`, innovation covariance'i +`S = P_pp + R_GPS` olarak hesaplanır. `NIS = r^T S^-1 r` değeri 3 DOF için +`16.266236` (%99.9) kapısını aşarsa ölçüm state'e dokunmadan reddedilir. +Kabul edilen update Joseph covariance formunu, quaternion error injection'ı +ve attitude reset Jacobian'ını kullanır. + +Default ESKF process/başlangıç değerleri: + +| Parametre | Default | +|---|---:| +| Accel noise density | 0.03 m/s2/sqrt(Hz), her eksen | +| Gyro noise density | 0.003 rad/s/sqrt(Hz), her eksen | +| Accel bias random walk | 0.002 m/s2/sqrt(s) | +| Gyro bias random walk | 0.002 deg/s/sqrt(s) | +| İlk yatay/düşey konum sigma | 10 m / 20 m | +| İlk hız sigma | 1 m/s | +| İlk roll-pitch/yaw sigma | 5 deg / 30 deg | +| Gravity | 9.80665 m/s2 | +| Maksimum propagation alt adımı | 20 ms | +| Konum NIS kapısı | 16.266236, 3 DOF %99.9 | + +### Asenkron veri akışı ve timestamp + +IMU ve GPS aynı local monotonic CPU/CLINT tick alanında timestamp edilir. +Filtre `dt` hesabında GPS week/TOW kullanmaz. GPS ölçümü WLS'nin bittiği anda +değil, ölçüm epoch interrupt'ında alınan local tick ile filtreye girer. WLS +geç geldiyse engine son GPS checkpoint'ine dönüp IMU geçmişini gerçek ölçüm +anına kadar oynatır, GPS update'i uygular ve bugüne kadar tekrar ilerler. + +Akışın ayrıntısı: + +1. TDK INT1 ISR'a girildiğinde `BSP_CLINT_MTIME` hemen okunur. +2. ISR sensörün 14-byte register burst'ünü alır ve yalnız raw integer değerleri + 256-elemanlı SPSC kuyruğa koyar; ISR içinde floating-point ESKF hesabı yoktur. +3. Foreground raw accel/gyro/temp değerlerini SI birimlerine çevirir ve aynı + local timestamp ile fusion engine'e verir. +4. Her uydu nav-solve event'i yakalandığında local tick event kaydına eklenir. + Coherent WLS epoch'u için geçerli uydu timestamp'lerinin medyanı seçilir. +5. WLS daha sonra tamamlansa bile GPS mesajı bu ölçüm-epoch timestamp'ini taşır. +6. Foreground önce bekleyen IMU'ları, sonra GPS kuyruğunu tüketir. Gecikmiş GPS + son checkpoint'ten kendi timestamp'ine kadar IMU replay edilerek uygulanır; + kalan IMU geçmişi tekrar oynatılarak current state yeniden oluşturulur. + +IMU history kapasitesi 2048 örnektir; 500 Hz'de yaklaşık 4.096 s fixed-lag +alanı sağlar. History dolarsa en eski IMU sessizce zaman aralığı kaybettirilerek +atılmaz: checkpoint önce o örnekle ilerletilir, sonra ring buffer'dan çıkarılır. +Gelecek timestamp'li GPS için sekiz-elemanlı sıralı pending queue bulunur; +checkpoint'ten eski ölçüm stale olarak reddedilir. + +GPS week/TOW uydu zamanı çözümü ve kanal coherence kontrolü için gereklidir, +ancak ESKF `dt` kaynağı değildir. Bu ayrım WLS computation latency'sinin IMU +zaman eksenini bozmasını önler. + +### Başlangıç kalibrasyonu, hareket kararı ve zero update + +Engine açılışta doğrudan RUNNING olmaz. Default olarak en az 2 s ve en az 50 +quiet IMU örneği ister. Tek örnekte gravity norm hatası 0.30 m/s2'yi veya gyro +normu 2 deg/s'yi aşarsa pencere sıfırlanır. Pencere sonunda her eksende accel +standard deviation 0.12 m/s2, gyro standard deviation 0.15 deg/s altında +olmalıdır. Gravity ortalaması roll/pitch'i, gyro ortalaması ilk gyro bias'ını +verir. Yaw gravity ile gözlenemediği için başlangıçta sıfır referanslı kalır. +Benzer şekilde stationary IMU ile yatay accel bias ve küçük tilt tamamen +ayrılamaz; sonraki GPS/kinematik update'ler bu error-state'leri düzeltir. + +Motion-state default eşikleri: + +| Karar parametresi | Değer | +|---|---:| +| Stationary lineer ivme üst sınırı | 0.15 m/s2 | +| Stationary gyro üst sınırı | 1 deg/s | +| Güçlü hareket accel eşiği | 0.40 m/s2 | +| Güçlü hareket gyro eşiği | 3 deg/s | +| Güçlü hareket jerk eşiği | 4 m/s3 | +| Jerk düşük-geçiren zaman sabiti | 100 ms | +| Stationary hız kapısı | 0.40 m/s | +| Moving hız kapısı | 0.50 m/s | +| Stationary'e dönüş quiet dwell | 1.0 s | +| Zayıf hareket doğrulaması | 3 ardışık IMU örneği | + +Güçlü accel/gyro/jerk veya yüksek ESKF hızı stationary state'ten tek örnekte +çıkabilir. Geri dönüş daha konservatiftir: quiet IMU, düşük ESKF hızı, aktif +GNSS-moving veto olmaması ve 1 s dwell birlikte gerekir. ZUPT ile zero-gyro +update yalnız gerçek `STATIONARY` state'inde, her IMU örneğinde değil 20 Hz'de +uygulanır. ZUPT velocity sigma 0.03 m/s, zero-gyro sigma eksen başına +0.01 deg/s'dir. + +GPS motion evidence, ardışık kabul edilmiş ENU konumları arasındaki yatay +displacement için 2 DOF `9.21034` (%99) NIS ve en az 0.50 m/s hız ister. Moving +kararı iki doğrulama ister. Bu metrik GPS position update NIS'inden ayrıdır; +biri hareket-state veto'su, diğeri ölçüm kabul/reject kapısıdır. + +GPS adapter ilk valid çözümü ENU origin yapar. WLS covariance yatayda en az +0.50 m, düşeyde en az 1.00 m sigma ile sınırlandırılır; korelasyonlar clamp +edilir ve covariance positive-definite değilse ESKF'ye ulaşmadan reddedilir. + +### WLS çıktısı ESKF'ye nasıl besleniyor? + +WLS çıktısı ESKF'ye gerçekten beslenmektedir. ESKF yalnız IMU ile serbest INS +çalıştırmıyor; her valid GPS epoch'unda WLS konumu ve WLS measurement +covariance'i ile position update yapıyor. Tam veri yolu şöyledir: + +```text +Uydu pseudorange ölçümleri + | + v +GPS WLS: konum + ENU covariance + residual/chi-square + validity + | + v +gps_fusion_publisher: ortak local CPU timestamp'li GPS mesajı + | + v +GPS adapter: LLH -> local ENU, covariance sanitize/floor + | + v +ESKF: GPS position innovation + NIS + Kalman update +``` + +WLS--ESKF sınırındaki alanlar: + +| WLS çıktısı | ESKF tarafındaki mevcut kullanım | +|---|---| +| Latitude/longitude/height | ENU position measurement'a çevrilip update'te kullanılır | +| 3x3 ENU covariance | GPS measurement covariance `R_GPS` olarak kullanılır | +| `solution_valid` | False ise GPS update yapılmaz | +| Measurement local timestamp | GPS update fixed-lag replay ile doğru epoch'ta uygulanır | +| `chi_square_ratio` | Mesajda taşınır ve source-quality üretilir; Kalman gain'i şu an sürekli ölçeklemez | +| `hAcc/vAcc` | Mesajda taşınır; ESKF ayrı scalar olarak değil tam 3x3 covariance'i kullanır | +| Residual RMS | Mesajda taşınır fakat ESKF update denkleminde şu an doğrudan kullanılmaz | +| Uydu başına post-fit residual | ESKF'ye tek tek gönderilmez; WLS integrity/FDE katmanında kalır | + +Burada iki farklı residual karıştırılmamalıdır: + +1. **WLS post-fit pseudorange residual'ı:** Her uydu için ölçülen pseudorange + ile çözülen geometri/clock modelinin farkıdır. WLS bu residual'lardan + `sum(weight_i * residual_i^2)`, DOF ve chi-square üretir. Chi-square fail + olursa `solution_valid=false` olur ve ölçüm ESKF tarafından kullanılmaz. +2. **ESKF GPS--INS innovation residual'ı:** WLS'nin ürettiği ENU konum ile + ESKF'nin IMU propagation sonucu tahmin ettiği ENU konumun farkıdır: + + ```text + r_ESKF = p_WLS_ENU - p_INS_ENU + S = P_ESKF_position + R_GPS_WLS + NIS = r_ESKF^T * S^-1 * r_ESKF + ``` + + ESKF bu NIS değeri 3 DOF %99.9 kapısını aşarsa upstream WLS valid olsa bile + GPS ölçümünü ikinci kez reddeder. + +Dolayısıyla iki covariance birbirinden farklıdır: + +- `P_ESKF`, IMU/INS error-state covariance'idir. IMU noise, gyro/accel bias + random walk, attitude, geçen `dt` ve önceki update'lerle ESKF içinde + propagation edilir. +- `R_GPS`, WLS position measurement covariance'idir. Şu anda + `(A^T * W_absolute * A)^-1` ile uydu geometrisi ve absolute measurement + weight'lerinden hesaplanıp ENU'ya çevrilir. + +WLS post-fit residual büyüklüğü mevcut kodda `R_GPS`'yi sürekli/adaptif olarak +şişirmez. Residual'ın mevcut doğrudan etkisi hard validity/chi-square kapısıdır; +accepted bir ölçümün Kalman ağırlığı esas olarak `P_ESKF` ve WLS'den gelen +`R_GPS` ile belirlenir. İleride absolute uydu sigma modeli gerçek loglarla +kalibre edildikten sonra reduced chi-square tabanlı, sınırlandırılmış bir +covariance inflation eklenebilir. Bunun için threshold'a bölünmüş mevcut +`chi_square_ratio` yerine istatistiksel ölçek olarak +`weighted_residual_sum / DOF` kullanılmalıdır. + +Hareket kararıyla ilgili iki koruma sentetik test sonucunda eklendi: + +- 200--500 Hz ham örnek farkı doğrudan jerk sayılmıyor; 100 ms zaman sabitli + düşük-geçiren ivmeden jerk üretiliyor. Böylece stationary IMU noise yanlış + hareket kararı üretmiyor, 1 m/s2 ani ivme yine ilk örnekte algılanıyor. +- Position-only GPS'in tek/iki gürültülü noktadan çıkardığı `still` kararı, + yüksek ESKF hızını ezip ZUPT başlatamıyor. GPS `moving` ek veto olarak + kullanılıyor; stationary hız kapısı 0.40 m/s, moving kapısı 0.50 m/s ile + histerezisli. İleride GPS Doppler velocity gelirse daha güçlü ve hızlı bir + duruş kanıtı olarak ayrıca eklenebilir. + +Linux/GCC testlerini çalıştırma: + +```sh +cd soc/software/fusion/tests +make clean +make test +make sanitize +make profile +``` + +`fusion_synthetic_test.c`, üretim kaynaklarıyla şu durumları birlikte sınar: + +- bias ve noise içeren stationary başlangıç kalibrasyonu; +- 100/200/500 Hz ve düzensiz local-tick IMU aralıkları; +- ani ivmelenme, sabit hız, ani fren, 90 derece yaw ve tekrar hareket; +- 0.8--1.2 s değişken GPS periodu ve 180--350 ms teslim gecikmesi; +- fixed-lag replay, covariance simetrisi/PSD ve uzun IMU geçmişi; +- iki adet yaklaşık 200 m sınıfı multipath/outlier ve NIS rejection; +- sekiz ayrı deterministic noise/timing seed'i; +- 30 s boyunca 500 Hz stationary IMU ve gecikmiş 1 Hz GPS. + +### Sentetik hareketli senaryo + +Hareketli senaryo 200 Hz IMU ile 60 s sürer. Ground-truth profil şöyledir: + +| Zaman | Ground truth hareket | +|---|---| +| 0--5 s | Stationary; ilk 2 s calibration alanı | +| 5--7 s | East yönünde +1.0 m/s2 ani ivmelenme | +| 7--10 s | East yönünde 2.0 m/s sabit hız | +| 10--12 s | East yönünde -1.0 m/s2 fren, tam duruş | +| 12--15 s | Stationary | +| 15--17 s | Yerinde +45 deg/s yaw, toplam 90 deg dönüş | +| 17--19 s | North yönünde +0.8 m/s2 ivmelenme | +| 19--22 s | North yönünde 1.6 m/s sabit hız | +| 22--24 s | North yönünde -0.8 m/s2 fren, tam duruş | +| 24--60 s | Stationary | + +Truth son konumu `[east=10 m, north=8 m, up=0 m]` ve son hızı sıfırdır. +Body-frame specific force, truth yaw ile ENU ivmesinin ters rotasyonundan +üretilir; dolayısıyla 90 derece dönüşten sonraki North ivmesi body X eksenine +doğru düşer ve attitude propagation da test edilir. + +Sentetik sensör modeli: + +| Girdi | Bias / noise | +|---|---| +| Accel bias | `[+0.030, -0.025, +0.060]` m/s2 | +| Gyro bias | `[+0.0010, -0.0007, +0.0009]` rad/s | +| Accel sample noise | Gaussian, 0.025 m/s2 sigma | +| Gyro sample noise | Gaussian, 0.0008 rad/s sigma | +| GPS yatay/düşey noise | 1.2 m / 2.0 m sigma | +| GPS measurement periodu | 0.8--1.2 s deterministic-random | +| WLS/teslim gecikmesi | 180--350 ms | +| GPS outlier | İki epoch'ta `[+150, -120, +80]` m; yaklaşık 208 m 3B | + +Outlier GPS mesajları source-valid bırakılır; böylece sadece upstream validity +değil ESKF'nin kendi NIS kapısı gerçekten sınanır. Her GPS farklı gecikmeyle +geldiği için current state'e doğrudan uygulanamaz ve fixed-lag replay yolu her +update'te çalışır. Sekiz seed IMU noise, GPS noise ve GPS period/delay dizisini +değiştirir; aynı seed her çalıştırmada aynı sonucu vererek regression testini +deterministic tutar. + +### Sentetik stationary senaryo + +İkinci uçtan uca senaryo 30 s boyunca 500 Hz IMU ve 1 Hz GPS kullanır. GPS +250 ms gecikmeli gelir; yatay/düşey sigma 0.8/1.5 m'dir. Accel bias +`[+0.020, -0.015, +0.050]` m/s2, gyro bias +`[+0.0008, -0.0005, +0.0006]` rad/s; sample noise sırasıyla 0.020 m/s2 ve +0.0006 rad/s'dir. Testin amacı yalnız position error değil, yüksek IMU rate'te +ham sample-to-sample noise'un jerk üzerinden sahte `MOVING` üretmediğini ve +20 Hz ZUPT'nin rate'ten bağımsız kaldığını göstermektir. + +### Unit-level ve invariant kontrolleri + +`eskf_motion_test.c` aşağıdaki kontrolleri ayrıca tek tek yapar: + +- düz ve tilt edilmiş stationary gravity propagation; +- normal GPS NIS accept ve büyük outlier reject; reject sonrası state'in + bit seviyesine yakın değişmemesi; +- ani hızlanma/fren, sabit hız sırasında stationary veto ve quiet dwell; +- stationary 500 Hz IMU noise altında false-motion olmaması; +- 100/200/500 Hz ve 3--17 ms düzensiz local-tick `dt` dizileri; +- 300 ms gecikmiş GPS replay sonucunun on-time filtre ile position, velocity, + quaternion, bias, 15x15 covariance ve motion state'te `1e-11` toleransta + aynı olması; +- future GPS pending, checkpoint'ten eski GPS stale reject; +- GPS kesintisinde 2048-history rollover ve checkpoint'in zaman kaybetmeden + ilerlemesi; +- LLH--ECEF--ENU round-trip, covariance floor/PD kontrolü; +- IMU/GPS SPSC queue FIFO sırası ve overflow sayaçları. + +Uçtan uca acceptance limitleri test kodunun içinde sabittir: hareketli yatay +RMSE `<2.0 m`, düşey RMSE `<2.5 m`, son yatay/düşey hata `<1.5/2.0 m`, son +hız `<0.05 m/s`, constant-velocity false-stationary örneği sıfır, bütün +outlier'lar reject ve covariance PSD olmalıdır. Stationary 500 Hz koşusunda +son yatay/düşey hata `<1/1 m`, son hız `<0.02 m/s` ve false-moving örneği +sıfır olmalıdır. Bunlar gerçek dünya accuracy garantisi değil, regression +fail sınırlarıdır. + +Son Linux/GCC `-O2` sonuçları: + +| Sentetik koşu | Sonuç | +|---|---:| +| 60 s hareketli yatay/düşey RMSE | 0.925 m / 0.427 m | +| 60 s son yatay/düşey hata | 0.306 m / 0.110 m | +| Moving algılama | 5.005 s, hareket başlangıcından 1 IMU örneği | +| Son duruşu stationary tanıma | 27.505 s, gerçek duruş 24.000 s | +| GPS accept/reject | 56 / 2 | +| Enjekte edilen/reddedilen outlier | 2 / 2 | +| Reddedilen GPS'in state sıçraması | 0.0 m | +| Sekiz seed en kötü yatay RMSE | 0.959 m | +| 30 s / 500 Hz stationary yanlış-moving örneği | 0 | +| 30 s / 500 Hz son hız | 0.0002 m/s | + +Baseline hareketli koşuda 58 delayed GPS update için 14,508 IMU örneği replay +edildi; 56 GPS kabul, iki sentetik outlier reject edildi. Toplam 749 rate-limitli +zero update uygulandı. Stationary 500 Hz koşusunda 27 GPS kabul edildi, 560 +zero update uygulandı. Final covariance Cholesky tabanlı PSD kontrolünü geçti +ve maksimum symmetry error test çözünürlüğünde sıfırdı. + +### Sentetik testin ortaya çıkardığı iki hata + +İlk sürümde jerk doğrudan iki ham accel örneğinin farkının `dt`'ye bölünmesiyle +üretiliyordu. 500 Hz'de küçük sample noise bile 4 m/s3 eşiğini sık geçebildiği +için stationary kartın motion state'i chatter edebilirdi. Jerk için 100 ms +low-pass state eklenince 10 s/500 Hz detector testi ve 30 s/500 Hz uçtan uca +test sıfır false-moving ile geçti; 1 m/s2 step yine ilk örnekte moving yaptı. + +İkinci ara sürüm, iki position-only GPS örneğinin `STILL` kararına ESKF hızını +ezme yetkisi veriyordu. 1.2 m GPS sigma ile 2 m/s sabit hareketin tek-epoch +displacement'i her zaman istatistiksel olarak güçlü değildir. Bazı seed'lerde +yanlış stationary kararı ZUPT'yi gerçek hareket sırasında açtı; sekiz-seed +testinde yatay RMSE 480 m ve son hata 1 km üzerini görerek regression'ı +yakaladı. Kalıcı düzeltmede GPS-still yüksek ESKF hızını artık geçersiz +kılamaz; GPS-moving yalnız ek veto olur. Düzeltme sonrası sekiz seed'in en kötü +yatay RMSE'si 0.959 m, false-stationary sayısı sıfır oldu. + +Bu sonuç GPS Doppler velocity'nin neden değerli olduğunu da gösterir. Position +farkından stationary/moving ayırımı düşük hızda ve kısa periodda zayıftır. +İleride GPS modülü güvenilir Doppler velocity ve covariance üretirse ayrı bir +velocity measurement update ve daha hızlı stop confirmation eklenmelidir. + +ASan ve UBSan hata vermedi. Sekiz adet 60 s hareketli koşu ile 30 s/500 Hz +stationary koşunun tamamı masaüstünde yaklaşık 0.70 s user CPU süresinde +tamamlandı. `gprof`te sürenin yaklaşık %73'ü covariance dahil ESKF +propagation, %18'i measurement/ZUPT update içindeydi. Bu yüzdeler darboğazı +gösterir; masaüstü süresi 50 MHz Rocket için doğrudan cycle tahmini değildir. +Profil koşusunda fixed-lag replay dahil 111,009 `fusion_engine_push_imu`, +239,075 covariance propagation step ve 30,352 selected measurement/ZUPT +update çağrısı oluştu. Host maksimum RSS yaklaşık 2.6 MB idi; bu değer glibc, +sanitizer/profiler ve process overhead içerdiğinden bare-metal BSS ile aynı +metrik değildir. + +Genesys2/RISC-V bare-metal build'i de geçti: + +```sh +make FPGA_BOARD=genesys2 fpga-elf-flash-image APP=gps_system \ + GPS_DEBUG=1 GPS_DEBUG_LEVEL=3 \ + TDK_IMU_ODR_HZ=500 GPS_NAV_SOLVE_TIMER_TICKS=50 +``` + +Son ELF boyutu text/data/bss olarak `103840 / 2944 / 204816` bayt, flash +payload `106784` bayt ve CRC32 `0x16dbc828` oldu. Kart bu test sırasında bağlı +olmadığı için SPI interrupt hızı, gerçek Rocket cycle yükü ve UART telemetry +henüz donanım üzerinde ölçülmedi. + +Build yeni fusion kaynaklarında compiler warning üretmedi. Linker'ın RAM +imajındaki mevcut `LOAD segment with RWX permissions` uyarısı devam ediyor; +bu ESKF değişikliğine özel değildir fakat ileride linker-script güvenlik/segment +ayrımı çalışmasında ele alınmalıdır. + +Kart tekrar bağlandığında sentetik testin yerine geçmeyen şu donanım +doğrulamaları yapılmalıdır: + +1. ICM-42688-P `WHO_AM_I=0x47`, gerçek accel/gyro eksen yönleri ve kart mounting + dönüşümü doğrulanmalı. +2. 100/200/500 Hz seçeneklerinde INT1 IRQ sayısı, sample sayısı, spurious IRQ, + estimated missed sample ve 256-raw-queue overflow sayaçları izlenmeli. +3. `navigation_fusion_service()` ve tek `eskf_propagate_step()` için Rocket + `rdcycle` ortalama/maksimum değerleri ölçülmeli; 50 MHz'deki gerçek CPU payı + bu cycle ölçümünden çıkarılmalı. +4. En az 2 s fiziksel stationary başlangıçta calibration reset sayısı, accel/ + gyro varyansı ve bulunan bias'lar sıcaklıkla birlikte loglanmalı. +5. Elle hareket testinde stationary -> moving çıkış süresi, sabit hızda ZUPT'nin + kapalı kaldığı ve durduktan sonra candidate -> stationary geçişi doğrulanmalı. +6. GPS WLS ölçüm timestamp'i ile IMU local tick'lerinin aynı CLINT domaininde + monoton kaldığı; replay/stale/pending/history-overflow sayaçlarının beklenen + değerlerde olduğu kontrol edilmeli. +7. Fused LLH/ENU, velocity, motion state, NIS, GPS accept/reject ve covariance + sigma değerleri UART'a kaydedilip fiziksel ground truth veya surveyed nokta + ile karşılaştırılmalı. + +Sentetik test; SPI elektrik bağlantısını, interrupt polarity/clear davranışını, +sensor scale/mounting hatasını, gerçek multipath dağılımını, sıcaklıkla bias +driftini veya 50 MHz timing deadline'ını kanıtlamaz. Buradaki metre sonuçları +gerçek ürün accuracy iddiası değil; bilinen truth ve covariance ile algoritma +tutarlılığı/regression sonucudur. + ## Standalone GPS WLS bütünlük çalışması (2026-07-12) Bu bölüm, standalone GPS konumundaki 20--30 m ve daha büyük sıçramaları diff --git a/soc/software/fusion/tests/Makefile b/soc/software/fusion/tests/Makefile index 5a213c5..a127c05 100644 --- a/soc/software/fusion/tests/Makefile +++ b/soc/software/fusion/tests/Makefile @@ -4,16 +4,46 @@ CFLAGS += -I../../gps_system/header LDLIBS ?= -lm GPS_TIME_SOURCE := ../../gps_system/src/gps_time_anchor.c +ESKF_SOURCES := ../../gps_system/src/eskf.c ../../gps_system/src/fusion_motion.c ../../gps_system/src/fusion_engine.c ../../gps_system/src/fusion_gps_adapter.c ../../gps_system/src/fusion_bus.c -.PHONY: all test benchmark clean +.PHONY: all test sanitize profile benchmark clean -all: standalone_gps_integrity_test +all: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE) $(CC) $(CFLAGS) $^ -o $@ $(LDLIBS) -test: standalone_gps_integrity_test +eskf_motion_test: eskf_motion_test.c $(ESKF_SOURCES) + $(CC) $(CFLAGS) $^ -o $@ $(LDLIBS) + +fusion_synthetic_test: fusion_synthetic_test.c $(ESKF_SOURCES) + $(CC) $(CFLAGS) $^ -o $@ $(LDLIBS) + +test: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test ./standalone_gps_integrity_test + ./eskf_motion_test + ./fusion_synthetic_test + +sanitize: + $(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \ + -fsanitize=address,undefined -fno-omit-frame-pointer \ + -I../../gps_system/header eskf_motion_test.c $(ESKF_SOURCES) \ + -o eskf_motion_test_san $(LDLIBS) + ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ + UBSAN_OPTIONS=halt_on_error=1 ./eskf_motion_test_san + $(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \ + -fsanitize=address,undefined -fno-omit-frame-pointer \ + -I../../gps_system/header fusion_synthetic_test.c $(ESKF_SOURCES) \ + -o fusion_synthetic_test_san $(LDLIBS) + ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ + UBSAN_OPTIONS=halt_on_error=1 ./fusion_synthetic_test_san + +profile: + $(CC) -std=c11 -O2 -g -pg -Wall -Wextra -Wpedantic \ + -I../../gps_system/header fusion_synthetic_test.c $(ESKF_SOURCES) \ + -o fusion_synthetic_test_gprof $(LDLIBS) + ./fusion_synthetic_test_gprof + gprof ./fusion_synthetic_test_gprof gmon.out benchmark: standalone_gps_integrity_test ./standalone_gps_integrity_test --bench-clean 100000 @@ -21,4 +51,4 @@ benchmark: standalone_gps_integrity_test ./standalone_gps_integrity_test --bench-double 100000 clean: - rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof gmon.out + rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace gmon.out diff --git a/soc/software/fusion/tests/eskf_motion_test.c b/soc/software/fusion/tests/eskf_motion_test.c new file mode 100644 index 0000000..015c7c6 --- /dev/null +++ b/soc/software/fusion/tests/eskf_motion_test.c @@ -0,0 +1,724 @@ +#include +#include +#include +#include + +#include "eskf.h" +#include "fusion_engine.h" +#include "fusion_bus.h" +#include "fusion_gps_adapter.h" +#include "fusion_motion.h" + +#define TEST_PI 3.14159265358979323846264338327950288 + +static int failures; + +#define CHECK(condition, message) do { \ + if(!(condition)) { \ + fprintf(stderr, "FAIL: %s\n", message); \ + failures++; \ + } \ +} while(0) + +static bool nearly_equal(double first, double second, double tolerance) +{ + return fabs(first - second) <= tolerance; +} + +static double test_norm3(const double value[3]) +{ + return sqrt(value[0] * value[0] + value[1] * value[1] + + value[2] * value[2]); +} + +static uint32_t noise_state = 0x9e3779b9u; + +static double uniform_noise(void) +{ + noise_state ^= noise_state << 13; + noise_state ^= noise_state >> 17; + noise_state ^= noise_state << 5; + return ((double)noise_state + 0.5) / 4294967296.0; +} + +static double gaussian_noise(void) +{ + double first = fmax(uniform_noise(), 1.0e-12); + double second = uniform_noise(); + return sqrt(-2.0 * log(first)) * cos(2.0 * TEST_PI * second); +} + +static void identity_filter(eskf_t* filter, eskf_config_t* config) +{ + eskf_default_config(config); + double position[3] = {0.0, 0.0, 0.0}; + double quaternion[4] = {1.0, 0.0, 0.0, 0.0}; + double accel_bias[3] = {0.0, 0.0, 0.0}; + double gyro_bias[3] = {0.0, 0.0, 0.0}; + CHECK(eskf_initialize(filter, config, position, quaternion, accel_bias, gyro_bias), + "ESKF identity initialization"); +} + +static void test_stationary_propagation(void) +{ + eskf_t filter; + eskf_config_t config; + identity_filter(&filter, &config); + double accel[3] = {0.0, 0.0, config.gravity_mps2}; + double gyro[3] = {0.0, 0.0, 0.0}; + for(unsigned int i = 0 ; i < 500u ; i++) + { + CHECK(eskf_propagate(&filter, &config, accel, gyro, 0.01), + "stationary propagation succeeds"); + } + CHECK(fabs(eskf_speed_mps(&filter)) < 1.0e-9, + "stationary propagation keeps zero velocity"); + CHECK(fabs(filter.position_m[2]) < 1.0e-9, + "stationary propagation keeps zero height"); + CHECK(eskf_state_is_finite(&filter), "stationary state remains finite"); +} + +static void test_gravity_alignment(void) +{ + eskf_t filter; + eskf_config_t config; + eskf_default_config(&config); + double measured_gravity[3] = {0.35, -0.22, 9.79793}; + double nav_up[3] = {0.0, 0.0, 1.0}; + double quaternion[4]; + CHECK(eskf_quaternion_align_vectors(measured_gravity, nav_up, quaternion), + "gravity alignment quaternion"); + double position[3] = {0.0, 0.0, 0.0}; + double accel_bias[3] = {0.0, 0.0, 0.0}; + double gyro_bias[3] = {0.0, 0.0, 0.0}; + double gravity_norm = sqrt(measured_gravity[0] * measured_gravity[0] + + measured_gravity[1] * measured_gravity[1] + + measured_gravity[2] * measured_gravity[2]); + for(unsigned int i = 0 ; i < 3u ; i++) + { + measured_gravity[i] *= config.gravity_mps2 / gravity_norm; + } + CHECK(eskf_initialize(&filter, &config, position, quaternion, + accel_bias, gyro_bias), + "tilted ESKF initialization"); + double gyro[3] = {0.0, 0.0, 0.0}; + CHECK(eskf_propagate(&filter, &config, measured_gravity, gyro, 0.01), + "tilted stationary propagation"); + CHECK(sqrt(filter.last_linear_accel_nav_mps2[0] * + filter.last_linear_accel_nav_mps2[0] + + filter.last_linear_accel_nav_mps2[1] * + filter.last_linear_accel_nav_mps2[1] + + filter.last_linear_accel_nav_mps2[2] * + filter.last_linear_accel_nav_mps2[2]) < 1.0e-8, + "gravity alignment removes gravity from linear acceleration"); +} + +static void test_position_nis_gate(void) +{ + eskf_t filter; + eskf_config_t config; + identity_filter(&filter, &config); + double covariance[9] = { + 4.0, 0.2, 0.0, + 0.2, 4.0, 0.0, + 0.0, 0.0, 9.0 + }; + double normal_position[3] = {1.0, -0.5, 0.2}; + double nis = NAN; + CHECK(eskf_position_update(&filter, &config, normal_position, + covariance, &nis) == ESKF_UPDATE_ACCEPTED, + "normal GPS position accepted"); + CHECK(isfinite(nis) && (nis < config.position_nis_gate), + "accepted GPS has finite in-gate NIS"); + double before[3]; + memcpy(before, filter.position_m, sizeof(before)); + double outlier[3] = {1000.0, -800.0, 500.0}; + CHECK(eskf_position_update(&filter, &config, outlier, + covariance, &nis) == ESKF_UPDATE_REJECTED_NIS, + "large GPS outlier rejected by NIS"); + CHECK(nearly_equal(before[0], filter.position_m[0], 1.0e-12) && + nearly_equal(before[1], filter.position_m[1], 1.0e-12) && + nearly_equal(before[2], filter.position_m[2], 1.0e-12), + "NIS rejection leaves nominal position unchanged"); + CHECK(filter.position_update_accepted_count == 1u && + filter.position_update_rejected_count == 1u, + "GPS accept/reject counters"); +} + +static double profile_acceleration(double time_s) +{ + if((time_s >= 2.0) && (time_s < 4.0)) + { + return 2.0; + } + if((time_s >= 6.0) && (time_s < 8.0)) + { + return -2.0; + } + return 0.0; +} + +static void test_motion_transitions(void) +{ + eskf_t filter; + eskf_config_t eskf_config; + identity_filter(&filter, &eskf_config); + fusion_motion_config_t motion_config; + fusion_motion_detector_t detector; + fusion_motion_default_config(&motion_config); + fusion_motion_init(&detector, true); + + const double dt_s = 0.01; + double first_moving_time = NAN; + double return_stationary_time = NAN; + bool stationary_during_constant_velocity = false; + bool deceleration_seen = false; + for(unsigned int sample = 1u ; sample <= 1100u ; sample++) + { + double time_s = (double)sample * dt_s; + double acceleration = profile_acceleration(time_s - 0.5 * dt_s); + double accel[3] = {acceleration, 0.0, eskf_config.gravity_mps2}; + double gyro[3] = {0.0, 0.0, 0.0}; + CHECK(eskf_propagate(&filter, &eskf_config, accel, gyro, dt_s), + "motion profile propagation"); + fusion_motion_state_t state = fusion_motion_update( + &detector, &motion_config, + filter.last_linear_accel_nav_mps2, + filter.last_unbiased_gyro_rps, + eskf_speed_mps(&filter), + FUSION_GNSS_MOTION_UNKNOWN, + dt_s); + if(isnan(first_moving_time) && (state == FUSION_MOTION_MOVING)) + { + first_moving_time = time_s; + } + if((time_s >= 4.0) && (time_s < 6.0) && + (state == FUSION_MOTION_STATIONARY)) + { + stationary_during_constant_velocity = true; + } + if((time_s >= 6.0) && (time_s < 8.0) && + (detector.last_linear_accel_norm_mps2 > 1.5)) + { + deceleration_seen = true; + } + if((time_s > 8.0) && isnan(return_stationary_time) && + (state == FUSION_MOTION_STATIONARY)) + { + return_stationary_time = time_s; + } + if(fusion_motion_allows_zero_update(&detector) && + (fmod(time_s, 0.05) < 0.5 * dt_s)) + { + CHECK(eskf_zero_velocity_update(&filter, 0.03), + "stationary ZUPT succeeds"); + } + } + + CHECK(isfinite(first_moving_time) && (first_moving_time <= 2.02), + "sudden acceleration exits stationary within one IMU interval"); + CHECK(!stationary_during_constant_velocity, + "constant velocity is not mistaken for stationary"); + CHECK(deceleration_seen, "sudden deceleration appears as dynamic evidence"); + CHECK(isfinite(return_stationary_time) && (return_stationary_time >= 9.0) && + (return_stationary_time <= 9.25), + "stationary re-entry waits for jerk settling and quiet dwell"); + CHECK(fabs(filter.velocity_mps[0]) < 0.03, + "accelerate/decelerate profile returns close to zero speed"); +} + +static double run_rate_profile(unsigned int imu_hz, double gps_period_s) +{ + eskf_t filter; + eskf_config_t config; + identity_filter(&filter, &config); + double dt_s = 1.0 / (double)imu_hz; + double next_gps_s = gps_period_s; + double truth_position = 0.0; + double truth_velocity = 0.0; + double gps_covariance[9] = { + 0.25, 0.0, 0.0, + 0.0, 0.25, 0.0, + 0.0, 0.0, 1.0 + }; + for(unsigned int sample = 0 ; sample < 6u * imu_hz ; sample++) + { + double time_s = (double)sample * dt_s; + double acceleration = 0.0; + if((time_s >= 1.0) && (time_s < 2.0)) + { + acceleration = 1.0; + } + else if((time_s >= 3.0) && (time_s < 4.0)) + { + acceleration = -1.0; + } + truth_position += truth_velocity * dt_s + 0.5 * acceleration * dt_s * dt_s; + truth_velocity += acceleration * dt_s; + double accel[3] = {acceleration, 0.0, config.gravity_mps2}; + double gyro[3] = {0.0, 0.0, 0.0}; + CHECK(eskf_propagate(&filter, &config, accel, gyro, dt_s), + "variable-rate propagation"); + double end_time_s = time_s + dt_s; + if((end_time_s + 1.0e-9) >= next_gps_s) + { + double gps_position[3] = {truth_position, 0.0, 0.0}; + CHECK(eskf_position_update(&filter, &config, gps_position, + gps_covariance, NULL) == ESKF_UPDATE_ACCEPTED, + "variable-rate asynchronous GPS update"); + next_gps_s += gps_period_s; + } + } + CHECK(fabs(truth_position - 2.0) < 1.0e-9, + "synthetic rate profile truth distance"); + return filter.position_m[0]; +} + +static void test_variable_rates(void) +{ + double position_100hz = run_rate_profile(100u, 1.0); + double position_200hz = run_rate_profile(200u, 0.5); + double position_500hz = run_rate_profile(500u, 1.0); + CHECK(fabs(position_100hz - 2.0) < 0.02, + "100 Hz IMU / 1 Hz GPS result"); + CHECK(fabs(position_200hz - 2.0) < 0.02, + "200 Hz IMU / 2 Hz GPS result"); + CHECK(fabs(position_500hz - 2.0) < 0.02, + "500 Hz IMU / 1 Hz GPS result"); + CHECK(fabs(position_100hz - position_200hz) < 0.02 && + fabs(position_200hz - position_500hz) < 0.02, + "filter result is insensitive to configured asynchronous rates"); +} + +static void test_irregular_imu_timestamps(void) +{ + eskf_t filter; + eskf_config_t config; + identity_filter(&filter, &config); + const double intervals_s[] = {0.003, 0.011, 0.004, 0.017, 0.006, 0.009}; + double elapsed_s = 0.0; + double truth_position[3] = {0.0, 0.0, 0.0}; + double truth_velocity[3] = {0.0, 0.0, 0.0}; + const double nav_acceleration[3] = {0.20, -0.10, 0.0}; + unsigned int interval_index = 0u; + while(elapsed_s < 5.0) + { + double dt_s = intervals_s[interval_index % + (sizeof(intervals_s) / sizeof(intervals_s[0]))]; + if((elapsed_s + dt_s) > 5.0) + { + dt_s = 5.0 - elapsed_s; + } + double accel[3] = { + nav_acceleration[0], nav_acceleration[1], config.gravity_mps2 + }; + double gyro[3] = {0.0, 0.0, 0.0}; + CHECK(eskf_propagate(&filter, &config, accel, gyro, dt_s), + "irregular timestamp propagation succeeds"); + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + truth_position[axis] += truth_velocity[axis] * dt_s + + 0.5 * nav_acceleration[axis] * dt_s * dt_s; + truth_velocity[axis] += nav_acceleration[axis] * dt_s; + } + elapsed_s += dt_s; + interval_index++; + } + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + CHECK(fabs(filter.position_m[axis] - truth_position[axis]) < 1.0e-10, + "irregular local-tick dt preserves position integration"); + CHECK(fabs(filter.velocity_mps[axis] - truth_velocity[axis]) < 1.0e-10, + "irregular local-tick dt preserves velocity integration"); + } +} + +static void test_motion_vetoes(void) +{ + fusion_motion_config_t config; + fusion_motion_detector_t detector; + fusion_motion_default_config(&config); + fusion_motion_init(&detector, false); + double zero[3] = {0.0, 0.0, 0.0}; + for(unsigned int i = 0 ; i < 200u ; i++) + { + (void)fusion_motion_update(&detector, &config, zero, zero, 3.0, + FUSION_GNSS_MOTION_UNKNOWN, 0.01); + } + CHECK(detector.state == FUSION_MOTION_MOVING, + "nonzero ESKF speed vetoes stationary at constant velocity"); + for(unsigned int i = 0 ; i < 200u ; i++) + { + (void)fusion_motion_update(&detector, &config, zero, zero, 3.0, + FUSION_GNSS_MOTION_STILL, 0.01); + } + CHECK(detector.state == FUSION_MOTION_MOVING, + "position-only GNSS still evidence cannot override high ESKF speed"); + for(unsigned int i = 0 ; i < 200u ; i++) + { + (void)fusion_motion_update(&detector, &config, zero, zero, 0.0, + FUSION_GNSS_MOTION_MOVING, 0.01); + } + CHECK(detector.state == FUSION_MOTION_MOVING, + "GNSS motion evidence vetoes stationary"); + for(unsigned int i = 0 ; i < 120u ; i++) + { + (void)fusion_motion_update(&detector, &config, zero, zero, 0.0, + FUSION_GNSS_MOTION_STILL, 0.01); + } + CHECK(detector.state == FUSION_MOTION_STATIONARY, + "quiet low-speed state enters stationary after dwell"); + + fusion_motion_init(&detector, true); + double weak_accel[3] = {0.25, 0.0, 0.0}; + for(unsigned int i = 0 ; i < config.weak_motion_samples - 1u ; i++) + { + (void)fusion_motion_update(&detector, &config, weak_accel, zero, 0.0, + FUSION_GNSS_MOTION_UNKNOWN, 0.01); + } + CHECK(detector.state == FUSION_MOTION_STATIONARY, + "short weak disturbance does not chatter state"); + (void)fusion_motion_update(&detector, &config, weak_accel, zero, 0.0, + FUSION_GNSS_MOTION_UNKNOWN, 0.01); + CHECK(detector.state == FUSION_MOTION_MOVING, + "persistent weak motion exits stationary"); +} + +static void test_stationary_noise_and_impulse_detection(void) +{ + fusion_motion_config_t config; + fusion_motion_detector_t detector; + fusion_motion_default_config(&config); + fusion_motion_init(&detector, true); + noise_state = 0x12345678u; + const double dt_s = 1.0 / 500.0; + unsigned int nonstationary_samples = 0u; + for(unsigned int sample = 0u ; sample < 5000u ; sample++) + { + double accel[3]; + double gyro[3]; + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + accel[axis] = 0.025 * gaussian_noise(); + gyro[axis] = 0.0008 * gaussian_noise(); + } + fusion_motion_state_t state = fusion_motion_update( + &detector, &config, accel, gyro, 0.0, + FUSION_GNSS_MOTION_STILL, dt_s); + if(state != FUSION_MOTION_STATIONARY) + { + nonstationary_samples++; + } + } + CHECK(nonstationary_samples == 0u, + "500 Hz stationary IMU noise does not cause false motion"); + + double sudden_accel[3] = {1.0, 0.0, 0.0}; + double zero_gyro[3] = {0.0, 0.0, 0.0}; + CHECK(fusion_motion_update(&detector, &config, sudden_accel, zero_gyro, + 0.0, FUSION_GNSS_MOTION_STILL, dt_s) == + FUSION_MOTION_MOVING, + "filtered jerk still detects a sudden acceleration in one 500 Hz sample"); +} + +static void initialize_engine_for_replay(fusion_engine_t* engine) +{ + fusion_engine_config_t config; + fusion_engine_default_config(&config, 1000u); + config.calibration_duration_s = 0.10; + config.calibration_min_samples = 10u; + CHECK(fusion_engine_init(engine, &config), "fusion engine initialization"); + + fusion_position_measurement_t origin = {0}; + origin.timestamp_ticks = 0u; + origin.source_valid = true; + origin.source_quality = 1.0; + origin.covariance_enu_m2[0] = 0.25; + origin.covariance_enu_m2[4] = 0.25; + origin.covariance_enu_m2[8] = 1.0; + CHECK(fusion_engine_push_position(engine, &origin, NULL, NULL), + "initial position retained during calibration"); + + for(uint64_t tick = 0u ; tick <= 100u ; tick += 10u) + { + fusion_imu_measurement_t imu = {0}; + imu.timestamp_ticks = tick; + imu.accel_mps2[2] = config.eskf.gravity_mps2; + imu.temperature_c = 25.0; + imu.sequence = (uint32_t)(tick / 10u); + CHECK(fusion_engine_push_imu(engine, &imu), + "stationary calibration IMU accepted"); + } + CHECK(engine->state == FUSION_ENGINE_RUNNING, + "fusion engine leaves calibration after quiet duration"); +} + +static double replay_profile_accel(uint64_t tick) +{ + double time_s = (double)tick / 1000.0; + if((time_s > 1.0) && (time_s <= 2.0)) + { + return 1.0; + } + if((time_s > 2.0) && (time_s <= 3.0)) + { + return -1.0; + } + return 0.0; +} + +static void push_replay_imu(fusion_engine_t* engine, uint64_t tick) +{ + fusion_imu_measurement_t imu = {0}; + imu.timestamp_ticks = tick; + imu.accel_mps2[0] = replay_profile_accel(tick); + imu.accel_mps2[2] = engine->config.eskf.gravity_mps2; + imu.temperature_c = 25.0; + imu.sequence = (uint32_t)(tick / 10u); + CHECK(fusion_engine_push_imu(engine, &imu), "replay profile IMU accepted"); +} + +static fusion_position_measurement_t replay_position(uint64_t tick, double east_m) +{ + fusion_position_measurement_t position = {0}; + position.timestamp_ticks = tick; + position.position_enu_m[0] = east_m; + position.covariance_enu_m2[0] = 0.25; + position.covariance_enu_m2[4] = 0.25; + position.covariance_enu_m2[8] = 1.0; + position.source_quality = 1.0; + position.source_valid = true; + return position; +} + +static void check_runtime_close(const fusion_runtime_t* first, + const fusion_runtime_t* second, + double tolerance) +{ + for(unsigned int i = 0 ; i < 3u ; i++) + { + CHECK(fabs(first->filter.position_m[i] - second->filter.position_m[i]) < tolerance, + "delayed replay position matches on-time update"); + CHECK(fabs(first->filter.velocity_mps[i] - second->filter.velocity_mps[i]) < tolerance, + "delayed replay velocity matches on-time update"); + CHECK(fabs(first->filter.accel_bias_mps2[i] - second->filter.accel_bias_mps2[i]) < tolerance, + "delayed replay accel bias matches on-time update"); + CHECK(fabs(first->filter.gyro_bias_rps[i] - second->filter.gyro_bias_rps[i]) < tolerance, + "delayed replay gyro bias matches on-time update"); + } + for(unsigned int i = 0 ; i < 4u ; i++) + { + CHECK(fabs(first->filter.body_to_nav_quaternion[i] - + second->filter.body_to_nav_quaternion[i]) < tolerance, + "delayed replay attitude matches on-time update"); + } + for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++) + { + CHECK(fabs(first->filter.covariance[row][col] - + second->filter.covariance[row][col]) < tolerance, + "delayed replay covariance matches on-time update"); + } + } + CHECK(first->motion.state == second->motion.state, + "delayed replay motion state matches on-time update"); + CHECK(first->filter_timestamp_ticks == second->filter_timestamp_ticks, + "delayed replay timestamp matches on-time update"); +} + +static void test_delayed_position_replay(void) +{ + static fusion_engine_t on_time; + static fusion_engine_t delayed; + initialize_engine_for_replay(&on_time); + initialize_engine_for_replay(&delayed); + fusion_position_measurement_t gps_at_1500 = replay_position(1500u, 0.125); + for(uint64_t tick = 110u ; tick <= 3200u ; tick += 10u) + { + push_replay_imu(&on_time, tick); + push_replay_imu(&delayed, tick); + if(tick == 1500u) + { + eskf_update_result_t result = ESKF_UPDATE_INVALID; + CHECK(fusion_engine_push_position(&on_time, &gps_at_1500, + &result, NULL), + "on-time position update processed"); + CHECK(result == ESKF_UPDATE_ACCEPTED, + "on-time position update accepted"); + } + if(tick == 1800u) + { + eskf_update_result_t result = ESKF_UPDATE_INVALID; + CHECK(fusion_engine_push_position(&delayed, &gps_at_1500, + &result, NULL), + "300 ms delayed position update replayed"); + CHECK(result == ESKF_UPDATE_ACCEPTED, + "delayed position update accepted at measurement epoch"); + check_runtime_close(&on_time.current, &delayed.current, 1.0e-11); + } + } + check_runtime_close(&on_time.current, &delayed.current, 1.0e-11); + CHECK(delayed.diagnostics.replay_count == 1u, + "one delayed position caused one fixed-lag replay"); + CHECK(delayed.diagnostics.replayed_imu_count > 0u, + "delayed position replayed stored IMU samples"); + + fusion_position_measurement_t future = replay_position(3500u, 1.0); + CHECK(fusion_engine_push_position(&delayed, &future, NULL, NULL), + "future-dated position queued without changing filter time"); + CHECK(delayed.pending_position_count == 1u, + "future position is pending until IMU reaches its timestamp"); + for(uint64_t tick = 3210u ; tick <= 3500u ; tick += 10u) + { + push_replay_imu(&delayed, tick); + } + CHECK(delayed.pending_position_count == 0u, + "pending position consumed at matching IMU time"); + CHECK(delayed.diagnostics.position_accepted == 2u, + "on-time and future position updates accepted"); + + fusion_position_measurement_t stale = replay_position(1400u, 0.1); + CHECK(!fusion_engine_push_position(&delayed, &stale, NULL, NULL), + "position older than last checkpoint rejected"); + CHECK(delayed.diagnostics.position_stale_rejected == 1u, + "stale position rejection is diagnosed"); +} + +static fusion_gps_measurement_t adapter_gps(double latitude_deg, + double longitude_deg, + double height_m) +{ + fusion_gps_measurement_t gps = {0}; + gps.latitude_deg = latitude_deg; + gps.longitude_deg = longitude_deg; + gps.height_m = height_m; + gps.covariance_enu_m2[0][0] = 0.04; + gps.covariance_enu_m2[1][1] = 0.09; + gps.covariance_enu_m2[2][2] = 0.25; + gps.chi_square_ratio = 0.5; + gps.solution_valid = true; + return gps; +} + +static void test_gps_adapter(void) +{ + fusion_gps_adapter_config_t config; + fusion_gps_adapter_t adapter; + fusion_gps_adapter_default_config(&config); + CHECK(fusion_gps_adapter_init(&adapter, &config), "GPS adapter initialization"); + fusion_gps_measurement_t origin = adapter_gps(41.015137, 28.979530, 100.0); + fusion_position_measurement_t local; + CHECK(fusion_gps_adapter_convert(&adapter, &origin, &local), + "GPS origin converts to local frame"); + CHECK(test_norm3(local.position_enu_m) < 1.0e-8, + "first valid GPS establishes zero ENU origin"); + CHECK(nearly_equal(local.covariance_enu_m2[0], 0.25, 1.0e-12) && + nearly_equal(local.covariance_enu_m2[4], 0.25, 1.0e-12) && + nearly_equal(local.covariance_enu_m2[8], 1.0, 1.0e-12), + "GPS covariance floors prevent overconfident ESKF updates"); + + fusion_gps_measurement_t moved = adapter_gps(41.015147, 28.979550, 103.0); + moved.timestamp_ticks = 1000u; + CHECK(fusion_gps_adapter_convert(&adapter, &moved, &local), + "second GPS converts to ENU"); + CHECK(local.position_enu_m[0] > 1.0 && local.position_enu_m[1] > 0.5, + "latitude/longitude change has expected ENU direction"); + double latitude; + double longitude; + double height; + CHECK(fusion_gps_adapter_local_to_geodetic(&adapter, local.position_enu_m, + &latitude, &longitude, &height), + "fused ENU converts back to geodetic telemetry"); + CHECK(fabs(latitude - moved.latitude_deg) < 1.0e-8 && + fabs(longitude - moved.longitude_deg) < 1.0e-8 && + fabs(height - moved.height_m) < 1.0e-3, + "ENU/geodetic round trip is accurate"); + + fusion_gps_measurement_t invalid = moved; + invalid.covariance_enu_m2[0][0] = -1.0; + CHECK(!fusion_gps_adapter_convert(&adapter, &invalid, &local), + "invalid GPS covariance is rejected before ESKF"); +} + +static void test_measurement_bus(void) +{ + static fusion_bus_t bus; + fusion_bus_init(&bus); + for(unsigned int sequence = 0u ; sequence < FUSION_IMU_QUEUE_CAPACITY - 1u ; sequence++) + { + fusion_imu_measurement_t imu = {0}; + imu.sequence = sequence; + imu.timestamp_ticks = sequence; + CHECK(fusion_bus_publish_imu(&bus, &imu), "IMU bus enqueue"); + } + fusion_imu_measurement_t overflow = {0}; + CHECK(!fusion_bus_publish_imu(&bus, &overflow), + "full IMU bus rejects newest sample without overwriting history"); + CHECK(bus.imu_overflow_count == 1u, "IMU bus overflow counter"); + for(unsigned int sequence = 0u ; sequence < FUSION_IMU_QUEUE_CAPACITY - 1u ; sequence++) + { + fusion_imu_measurement_t imu; + CHECK(fusion_bus_take_imu(&bus, &imu), "IMU bus dequeue"); + CHECK(imu.sequence == sequence, "IMU bus preserves FIFO ordering"); + } + CHECK(!fusion_bus_take_imu(&bus, &overflow), "empty IMU bus reports no sample"); + + for(unsigned int sequence = 0u ; sequence < FUSION_GPS_QUEUE_CAPACITY - 1u ; sequence++) + { + fusion_gps_measurement_t gps = {0}; + gps.timestamp_ticks = sequence; + CHECK(fusion_bus_publish_gps(&bus, &gps), "GPS bus enqueue"); + } + fusion_gps_measurement_t gps_overflow = {0}; + CHECK(!fusion_bus_publish_gps(&bus, &gps_overflow), "GPS bus overflow detected"); + CHECK(bus.gps_overflow_count == 1u, "GPS bus overflow counter"); +} + +static void test_history_rolls_checkpoint_forward(void) +{ + static fusion_engine_t engine; + initialize_engine_for_replay(&engine); + uint64_t tick = 110u; + for(unsigned int i = 0u ; i < FUSION_IMU_HISTORY_CAPACITY + 64u ; i++, tick += 10u) + { + push_replay_imu(&engine, tick); + } + CHECK(engine.state == FUSION_ENGINE_RUNNING, + "long GPS outage does not fault fusion engine"); + CHECK(engine.diagnostics.imu_history_overflow == 64u, + "fixed-lag history reports bounded rollover"); + CHECK(engine.diagnostics.checkpoint_advance_count == 64u, + "checkpoint advances with each expired IMU sample"); + CHECK(engine.checkpoint.filter_timestamp_ticks > 100u, + "checkpoint no longer precedes retained history"); + fusion_position_measurement_t recovery = replay_position(tick - 10u, 0.0); + eskf_update_result_t result = ESKF_UPDATE_INVALID; + CHECK(fusion_engine_push_position(&engine, &recovery, &result, NULL), + "GPS recovery after history rollover is processed"); + CHECK(result == ESKF_UPDATE_ACCEPTED, + "GPS recovery after long outage passes consistent NIS"); +} + +int main(void) +{ + test_stationary_propagation(); + test_gravity_alignment(); + test_position_nis_gate(); + test_motion_transitions(); + test_variable_rates(); + test_irregular_imu_timestamps(); + test_motion_vetoes(); + test_stationary_noise_and_impulse_detection(); + test_delayed_position_replay(); + test_gps_adapter(); + test_measurement_bus(); + test_history_rolls_checkpoint_forward(); + if(failures != 0) + { + fprintf(stderr, "%d ESKF/motion test(s) failed\n", failures); + return 1; + } + printf("ESKF/motion tests passed (gravity, NIS, replay, noise, 100/200/500 Hz).\n"); + return 0; +} diff --git a/soc/software/fusion/tests/fusion_synthetic_test.c b/soc/software/fusion/tests/fusion_synthetic_test.c new file mode 100644 index 0000000..2b5f24c --- /dev/null +++ b/soc/software/fusion/tests/fusion_synthetic_test.c @@ -0,0 +1,648 @@ +#include +#include +#include +#include +#include + +#include "fusion_engine.h" + +#define SYNTH_PI 3.14159265358979323846264338327950288 +#define SYNTH_TIMEBASE_HZ 1000000u +#define SYNTH_IMU_HZ 200u +#define SYNTH_DURATION_S 60u +#define SYNTH_GPS_EVENT_CAPACITY 96u + +typedef struct { + uint32_t state; +} synth_rng_t; + +typedef struct { + double position_m[3]; + double velocity_mps[3]; + double yaw_rad; +} synth_truth_t; + +typedef struct { + fusion_position_measurement_t measurement; + uint64_t delivery_ticks; + bool injected_outlier; +} synth_gps_event_t; + +typedef struct { + double horizontal_rmse_m; + double vertical_rmse_m; + double final_horizontal_error_m; + double final_vertical_error_m; + double final_speed_mps; + double max_rejected_state_jump_m; + double covariance_symmetry_error; + uint64_t accepted_gps; + uint64_t rejected_gps; + uint64_t replay_count; + uint64_t replayed_imu_count; + uint64_t zero_updates; + unsigned int injected_outliers; + unsigned int rejected_injected_outliers; + unsigned int false_stationary_samples; + double moving_detection_time_s; + double stationary_reentry_time_s; + bool covariance_positive_semidefinite; + bool state_finite; +} synth_metrics_t; + +typedef struct { + double final_horizontal_error_m; + double final_vertical_error_m; + double final_speed_mps; + uint64_t accepted_gps; + uint64_t zero_updates; + unsigned int nonstationary_samples; + bool covariance_positive_semidefinite; + bool state_finite; +} stationary_metrics_t; + +static int failures; + +#define CHECK(condition, message) do { \ + if(!(condition)) { \ + fprintf(stderr, "FAIL: %s\n", message); \ + failures++; \ + } \ +} while(0) + +static double square(double value) +{ + return value * value; +} + +static double norm2(double first, double second) +{ + return sqrt(first * first + second * second); +} + +static double norm3(const double value[3]) +{ + return sqrt(square(value[0]) + square(value[1]) + square(value[2])); +} + +static uint32_t rng_next(synth_rng_t* rng) +{ + uint32_t value = rng->state; + value ^= value << 13; + value ^= value >> 17; + value ^= value << 5; + rng->state = value; + return value; +} + +static double rng_uniform(synth_rng_t* rng) +{ + return ((double)rng_next(rng) + 0.5) / 4294967296.0; +} + +static double rng_gaussian(synth_rng_t* rng) +{ + double first = fmax(rng_uniform(rng), 1.0e-12); + double second = rng_uniform(rng); + return sqrt(-2.0 * log(first)) * cos(2.0 * SYNTH_PI * second); +} + +static void trajectory_input(double time_s, + double acceleration_nav_mps2[3], + double* yaw_rate_rps) +{ + memset(acceleration_nav_mps2, 0, 3u * sizeof(double)); + *yaw_rate_rps = 0.0; + if((time_s >= 5.0) && (time_s < 7.0)) + { + acceleration_nav_mps2[0] = 1.0; + } + else if((time_s >= 10.0) && (time_s < 12.0)) + { + acceleration_nav_mps2[0] = -1.0; + } + else if((time_s >= 15.0) && (time_s < 17.0)) + { + *yaw_rate_rps = 0.25 * SYNTH_PI; + } + else if((time_s >= 17.0) && (time_s < 19.0)) + { + acceleration_nav_mps2[1] = 0.8; + } + else if((time_s >= 22.0) && (time_s < 24.0)) + { + acceleration_nav_mps2[1] = -0.8; + } +} + +static void truth_step(synth_truth_t* truth, + double dt_s, + const double acceleration_nav_mps2[3], + double yaw_rate_rps) +{ + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + truth->position_m[axis] += truth->velocity_mps[axis] * dt_s + + 0.5 * acceleration_nav_mps2[axis] * dt_s * dt_s; + truth->velocity_mps[axis] += acceleration_nav_mps2[axis] * dt_s; + } + truth->yaw_rad += yaw_rate_rps * dt_s; +} + +static fusion_position_measurement_t make_position( + uint64_t timestamp_ticks, + const synth_truth_t* truth, + synth_rng_t* gps_rng, + bool outlier) +{ + fusion_position_measurement_t position; + memset(&position, 0, sizeof(position)); + position.timestamp_ticks = timestamp_ticks; + position.position_enu_m[0] = truth->position_m[0] + 1.2 * rng_gaussian(gps_rng); + position.position_enu_m[1] = truth->position_m[1] + 1.2 * rng_gaussian(gps_rng); + position.position_enu_m[2] = truth->position_m[2] + 2.0 * rng_gaussian(gps_rng); + if(outlier) + { + position.position_enu_m[0] += 150.0; + position.position_enu_m[1] -= 120.0; + position.position_enu_m[2] += 80.0; + } + position.covariance_enu_m2[0] = square(1.2); + position.covariance_enu_m2[4] = square(1.2); + position.covariance_enu_m2[8] = square(2.0); + position.source_quality = outlier ? 0.7 : 1.0; + position.source_valid = true; + return position; +} + +static bool covariance_is_psd(const eskf_t* filter, + double* symmetry_error_out) +{ + double lower[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}}; + double max_symmetry_error = 0.0; + for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = 0u ; col < ESKF_ERROR_STATE_DIM ; col++) + { + double error = fabs(filter->covariance[row][col] - + filter->covariance[col][row]); + if(error > max_symmetry_error) + { + max_symmetry_error = error; + } + } + } + if(symmetry_error_out != NULL) + { + *symmetry_error_out = max_symmetry_error; + } + + /* A tiny diagonal jitter makes the semidefinite check numerically robust. */ + for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = 0u ; col <= row ; col++) + { + double value = filter->covariance[row][col]; + if(row == col) + { + value += 1.0e-10; + } + for(unsigned int k = 0u ; k < col ; k++) + { + value -= lower[row][k] * lower[col][k]; + } + if(row == col) + { + if(!isfinite(value) || (value <= 0.0)) + { + return false; + } + lower[row][col] = sqrt(value); + } + else + { + lower[row][col] = value / lower[col][col]; + } + } + } + return true; +} + +static void initialize_synthetic_engine(fusion_engine_t* engine) +{ + fusion_engine_config_t config; + fusion_engine_default_config(&config, SYNTH_TIMEBASE_HZ); + config.calibration_duration_s = 2.0; + config.calibration_min_samples = SYNTH_IMU_HZ; + CHECK(fusion_engine_init(engine, &config), "synthetic fusion engine initializes"); + + fusion_position_measurement_t origin; + memset(&origin, 0, sizeof(origin)); + origin.timestamp_ticks = 0u; + origin.covariance_enu_m2[0] = square(1.2); + origin.covariance_enu_m2[4] = square(1.2); + origin.covariance_enu_m2[8] = square(2.0); + origin.source_quality = 1.0; + origin.source_valid = true; + CHECK(fusion_engine_push_position(engine, &origin, NULL, NULL), + "initial GPS is retained during calibration"); +} + +static synth_metrics_t run_dynamic_scenario(uint32_t scenario_seed) +{ + static fusion_engine_t engine; + initialize_synthetic_engine(&engine); + synth_truth_t truth = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, 0.0}; + synth_rng_t imu_rng = {0x13579bdfu ^ (0x9e3779b9u * scenario_seed)}; + synth_rng_t gps_rng = {0x2468ace1u ^ (0x85ebca6bu * scenario_seed)}; + synth_rng_t timing_rng = {0xa5a5c3c3u ^ (0xc2b2ae35u * scenario_seed)}; + const double accel_bias[3] = {0.030, -0.025, 0.060}; + const double gyro_bias[3] = {0.0010, -0.0007, 0.0009}; + const uint64_t imu_period_ticks = SYNTH_TIMEBASE_HZ / SYNTH_IMU_HZ; + const double dt_s = 1.0 / (double)SYNTH_IMU_HZ; + const uint64_t final_tick = (uint64_t)SYNTH_DURATION_S * SYNTH_TIMEBASE_HZ; + uint64_t next_gps_tick = 3000000u; + unsigned int gps_sequence = 0u; + synth_gps_event_t events[SYNTH_GPS_EVENT_CAPACITY]; + unsigned int event_head = 0u; + unsigned int event_count = 0u; + double horizontal_error_sum = 0.0; + double vertical_error_sum = 0.0; + uint64_t error_sample_count = 0u; + synth_metrics_t metrics; + memset(&metrics, 0, sizeof(metrics)); + metrics.moving_detection_time_s = NAN; + metrics.stationary_reentry_time_s = NAN; +#ifdef SYNTH_TRACE + fusion_motion_state_t traced_state = FUSION_MOTION_BOOTSTRAP; +#endif + + for(uint64_t tick = 0u ; tick <= final_tick ; tick += imu_period_ticks) + { + double acceleration_nav[3] = {0.0, 0.0, 0.0}; + double yaw_rate = 0.0; + if(tick != 0u) + { + double midpoint_s = ((double)tick - 0.5 * (double)imu_period_ticks) / + (double)SYNTH_TIMEBASE_HZ; + trajectory_input(midpoint_s, acceleration_nav, &yaw_rate); + truth_step(&truth, dt_s, acceleration_nav, yaw_rate); + } + + double midpoint_yaw = truth.yaw_rad - 0.5 * yaw_rate * dt_s; + double cosine = cos(midpoint_yaw); + double sine = sin(midpoint_yaw); + double specific_force_nav[3] = { + acceleration_nav[0], acceleration_nav[1], engine.config.eskf.gravity_mps2 + }; + fusion_imu_measurement_t imu; + memset(&imu, 0, sizeof(imu)); + imu.timestamp_ticks = tick; + imu.accel_mps2[0] = cosine * specific_force_nav[0] + + sine * specific_force_nav[1]; + imu.accel_mps2[1] = -sine * specific_force_nav[0] + + cosine * specific_force_nav[1]; + imu.accel_mps2[2] = specific_force_nav[2]; + imu.gyro_rps[2] = yaw_rate; + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + imu.accel_mps2[axis] += accel_bias[axis] + 0.025 * rng_gaussian(&imu_rng); + imu.gyro_rps[axis] += gyro_bias[axis] + 0.0008 * rng_gaussian(&imu_rng); + } + imu.temperature_c = 25.0 + 0.5 * sin((double)tick / 10000000.0); + imu.sequence = (uint32_t)(tick / imu_period_ticks); + CHECK(fusion_engine_push_imu(&engine, &imu), "dynamic synthetic IMU accepted"); + if(engine.state == FUSION_ENGINE_FAULT) + { + break; + } + + if((tick >= next_gps_tick) && + ((event_head + event_count) < SYNTH_GPS_EVENT_CAPACITY)) + { + bool outlier = (gps_sequence == 8u) || (gps_sequence == 27u); + unsigned int index = event_head + event_count; + events[index].measurement = make_position(tick, &truth, &gps_rng, outlier); + double delay_s = 0.18 + 0.17 * rng_uniform(&timing_rng); + events[index].delivery_ticks = tick + + (uint64_t)(delay_s * (double)SYNTH_TIMEBASE_HZ + 0.5); + events[index].injected_outlier = outlier; + event_count++; + if(outlier) + { + metrics.injected_outliers++; + } + gps_sequence++; + double next_period_s = 0.80 + 0.40 * rng_uniform(&timing_rng); + next_gps_tick = tick + + (uint64_t)(next_period_s * (double)SYNTH_TIMEBASE_HZ + 0.5); + } + + while((event_count != 0u) && (events[event_head].delivery_ticks <= tick)) + { + double before[3]; + memcpy(before, engine.current.filter.position_m, sizeof(before)); + eskf_update_result_t result = ESKF_UPDATE_INVALID; + double nis = NAN; + CHECK(fusion_engine_push_position(&engine, &events[event_head].measurement, + &result, &nis), + "delayed asynchronous GPS is processed"); + if(events[event_head].injected_outlier && + (result == ESKF_UPDATE_REJECTED_NIS)) + { + metrics.rejected_injected_outliers++; + double state_jump = norm2( + engine.current.filter.position_m[0] - before[0], + engine.current.filter.position_m[1] - before[1]); + if(state_jump > metrics.max_rejected_state_jump_m) + { + metrics.max_rejected_state_jump_m = state_jump; + } + } + event_head++; + event_count--; + } + + if(engine.state == FUSION_ENGINE_RUNNING) + { + double time_s = (double)tick / (double)SYNTH_TIMEBASE_HZ; + fusion_motion_state_t state = engine.current.motion.state; +#ifdef SYNTH_TRACE + if(state != traced_state) + { + printf("TRACE t=%.3f state=%s speed=%.3f gnss=%u confirm=%u accel=%.3f jerk=%.3f\n", + time_s, fusion_motion_state_name(state), + eskf_speed_mps(&engine.current.filter), + (unsigned int)engine.current.gnss_evidence, + engine.current.gnss_moving_count, + engine.current.motion.last_linear_accel_norm_mps2, + engine.current.motion.last_jerk_norm_mps3); + traced_state = state; + } +#endif + if(isnan(metrics.moving_detection_time_s) && (time_s >= 5.0) && + (state == FUSION_MOTION_MOVING)) + { + metrics.moving_detection_time_s = time_s; + } + if((time_s >= 7.25) && (time_s <= 9.75) && + (state == FUSION_MOTION_STATIONARY)) + { + metrics.false_stationary_samples++; + } + if((time_s >= 19.25) && (time_s <= 21.75) && + (state == FUSION_MOTION_STATIONARY)) + { + metrics.false_stationary_samples++; + } + if(isnan(metrics.stationary_reentry_time_s) && (time_s >= 24.0) && + (state == FUSION_MOTION_STATIONARY)) + { + metrics.stationary_reentry_time_s = time_s; + } + if(time_s >= 3.0) + { + double east_error = engine.current.filter.position_m[0] - truth.position_m[0]; + double north_error = engine.current.filter.position_m[1] - truth.position_m[1]; + double up_error = engine.current.filter.position_m[2] - truth.position_m[2]; + horizontal_error_sum += square(east_error) + square(north_error); + vertical_error_sum += square(up_error); + error_sample_count++; + } + } + } + + metrics.accepted_gps = engine.diagnostics.position_accepted; + metrics.rejected_gps = engine.diagnostics.position_nis_rejected; + metrics.replay_count = engine.diagnostics.replay_count; + metrics.replayed_imu_count = engine.diagnostics.replayed_imu_count; + metrics.zero_updates = engine.diagnostics.zero_velocity_update_count; + metrics.state_finite = eskf_state_is_finite(&engine.current.filter); + metrics.covariance_positive_semidefinite = covariance_is_psd( + &engine.current.filter, &metrics.covariance_symmetry_error); + if(error_sample_count != 0u) + { + metrics.horizontal_rmse_m = sqrt(horizontal_error_sum / (double)error_sample_count); + metrics.vertical_rmse_m = sqrt(vertical_error_sum / (double)error_sample_count); + } + metrics.final_horizontal_error_m = norm2( + engine.current.filter.position_m[0] - truth.position_m[0], + engine.current.filter.position_m[1] - truth.position_m[1]); + metrics.final_vertical_error_m = fabs( + engine.current.filter.position_m[2] - truth.position_m[2]); + metrics.final_speed_mps = norm3(engine.current.filter.velocity_mps); + return metrics; +} + +static void check_dynamic_metrics(const synth_metrics_t* metrics) +{ + CHECK(metrics->state_finite, "60 s synthetic state remains finite"); + CHECK(metrics->covariance_positive_semidefinite, + "60 s covariance remains positive semidefinite"); + CHECK(metrics->covariance_symmetry_error < 1.0e-11, + "60 s covariance remains symmetric"); + CHECK(isfinite(metrics->moving_detection_time_s) && + (metrics->moving_detection_time_s <= 5.010), + "sudden acceleration is detected within two 200 Hz samples"); + CHECK(metrics->false_stationary_samples == 0u, + "constant-velocity legs never enable stationary updates"); + CHECK(isfinite(metrics->stationary_reentry_time_s) && + (metrics->stationary_reentry_time_s >= 25.0) && + (metrics->stationary_reentry_time_s <= 28.0), + "final stop enters stationary after hysteresis dwell"); + CHECK(metrics->injected_outliers == 2u, + "two synthetic multipath-like GPS outliers were injected"); + CHECK(metrics->rejected_injected_outliers == metrics->injected_outliers, + "NIS rejects every injected GPS outlier"); + CHECK(metrics->max_rejected_state_jump_m < 1.0e-8, + "rejected delayed GPS does not perturb the current state"); + CHECK(metrics->accepted_gps >= 45u, + "most asynchronous GPS measurements are accepted"); + CHECK(metrics->rejected_gps >= metrics->injected_outliers, + "GPS reject diagnostics include injected outliers"); + CHECK(metrics->replay_count >= 50u && metrics->replayed_imu_count > 1000u, + "delayed GPS exercises fixed-lag replay extensively"); + CHECK(metrics->zero_updates > 300u, + "stationary periods apply rate-limited ZUPT updates"); + CHECK(metrics->horizontal_rmse_m < 2.0, + "bias/noise/delay trajectory horizontal RMSE is bounded"); + CHECK(metrics->vertical_rmse_m < 2.5, + "bias/noise/delay trajectory vertical RMSE is bounded"); + CHECK(metrics->final_horizontal_error_m < 1.5, + "final stationary horizontal error is bounded"); + CHECK(metrics->final_vertical_error_m < 2.0, + "final stationary vertical error is bounded"); + CHECK(metrics->final_speed_mps < 0.05, + "final stationary ZUPT speed is near zero"); +} + +static stationary_metrics_t run_stationary_500hz_scenario(void) +{ + static fusion_engine_t engine; + fusion_engine_config_t config; + fusion_engine_default_config(&config, SYNTH_TIMEBASE_HZ); + config.calibration_duration_s = 2.0; + config.calibration_min_samples = 500u; + CHECK(fusion_engine_init(&engine, &config), "500 Hz stationary engine initializes"); + + fusion_position_measurement_t origin; + memset(&origin, 0, sizeof(origin)); + origin.covariance_enu_m2[0] = square(0.8); + origin.covariance_enu_m2[4] = square(0.8); + origin.covariance_enu_m2[8] = square(1.5); + origin.source_quality = 1.0; + origin.source_valid = true; + CHECK(fusion_engine_push_position(&engine, &origin, NULL, NULL), + "500 Hz stationary origin accepted"); + + const uint64_t imu_period_ticks = SYNTH_TIMEBASE_HZ / 500u; + const uint64_t final_tick = 30000000u; + uint64_t next_gps_tick = 3000000u; + uint64_t delivery_tick = 0u; + bool gps_pending = false; + fusion_position_measurement_t pending_gps; + synth_rng_t imu_rng = {0x10203040u}; + synth_rng_t gps_rng = {0x55667788u}; + const double accel_bias[3] = {0.020, -0.015, 0.050}; + const double gyro_bias[3] = {0.0008, -0.0005, 0.0006}; + stationary_metrics_t metrics; + memset(&metrics, 0, sizeof(metrics)); + + for(uint64_t tick = 0u ; tick <= final_tick ; tick += imu_period_ticks) + { + fusion_imu_measurement_t imu; + memset(&imu, 0, sizeof(imu)); + imu.timestamp_ticks = tick; + imu.accel_mps2[2] = config.eskf.gravity_mps2; + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + imu.accel_mps2[axis] += accel_bias[axis] + 0.020 * rng_gaussian(&imu_rng); + imu.gyro_rps[axis] = gyro_bias[axis] + 0.0006 * rng_gaussian(&imu_rng); + } + imu.temperature_c = 24.0; + imu.sequence = (uint32_t)(tick / imu_period_ticks); + CHECK(fusion_engine_push_imu(&engine, &imu), "500 Hz stationary IMU accepted"); + + if((tick >= next_gps_tick) && !gps_pending) + { + synth_truth_t stationary_truth = {{0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, 0.0}; + pending_gps = make_position(tick, &stationary_truth, &gps_rng, false); + pending_gps.position_enu_m[0] *= 0.8 / 1.2; + pending_gps.position_enu_m[1] *= 0.8 / 1.2; + pending_gps.position_enu_m[2] *= 1.5 / 2.0; + pending_gps.covariance_enu_m2[0] = square(0.8); + pending_gps.covariance_enu_m2[4] = square(0.8); + pending_gps.covariance_enu_m2[8] = square(1.5); + delivery_tick = tick + 250000u; + gps_pending = true; + next_gps_tick += 1000000u; + } + if(gps_pending && (tick >= delivery_tick)) + { + eskf_update_result_t result = ESKF_UPDATE_INVALID; + CHECK(fusion_engine_push_position(&engine, &pending_gps, &result, NULL), + "500 Hz delayed stationary GPS processed"); + CHECK(result == ESKF_UPDATE_ACCEPTED, + "500 Hz stationary GPS passes NIS"); + gps_pending = false; + } + if((tick >= 3000000u) && (engine.state == FUSION_ENGINE_RUNNING) && + (engine.current.motion.state != FUSION_MOTION_STATIONARY)) + { + metrics.nonstationary_samples++; + } + } + + metrics.accepted_gps = engine.diagnostics.position_accepted; + metrics.zero_updates = engine.diagnostics.zero_velocity_update_count; + metrics.state_finite = eskf_state_is_finite(&engine.current.filter); + metrics.covariance_positive_semidefinite = covariance_is_psd( + &engine.current.filter, NULL); + metrics.final_horizontal_error_m = norm2(engine.current.filter.position_m[0], + engine.current.filter.position_m[1]); + metrics.final_vertical_error_m = fabs(engine.current.filter.position_m[2]); + metrics.final_speed_mps = norm3(engine.current.filter.velocity_mps); + return metrics; +} + +static void check_stationary_metrics(const stationary_metrics_t* metrics) +{ + CHECK(metrics->state_finite, "30 s / 500 Hz stationary state remains finite"); + CHECK(metrics->covariance_positive_semidefinite, + "30 s / 500 Hz stationary covariance remains PSD"); + CHECK(metrics->nonstationary_samples == 0u, + "30 s / 500 Hz noise never creates a false moving state"); + CHECK(metrics->accepted_gps >= 26u, + "30 s / 500 Hz delayed stationary GPS remains accepted"); + CHECK(metrics->zero_updates >= 500u, + "30 s / 500 Hz stationary run keeps rate-limited ZUPT active"); + CHECK(metrics->final_horizontal_error_m < 1.0, + "30 s / 500 Hz stationary horizontal error is bounded"); + CHECK(metrics->final_vertical_error_m < 1.0, + "30 s / 500 Hz stationary vertical error is bounded"); + CHECK(metrics->final_speed_mps < 0.02, + "30 s / 500 Hz stationary speed remains near zero"); +} + +int main(void) +{ + synth_metrics_t metrics = run_dynamic_scenario(0u); + check_dynamic_metrics(&metrics); + double worst_horizontal_rmse_m = metrics.horizontal_rmse_m; + double worst_final_horizontal_error_m = metrics.final_horizontal_error_m; + double latest_stationary_reentry_s = metrics.stationary_reentry_time_s; + for(uint32_t scenario = 1u ; scenario < 8u ; scenario++) + { + synth_metrics_t trial = run_dynamic_scenario(scenario); + check_dynamic_metrics(&trial); + worst_horizontal_rmse_m = fmax(worst_horizontal_rmse_m, + trial.horizontal_rmse_m); + worst_final_horizontal_error_m = fmax(worst_final_horizontal_error_m, + trial.final_horizontal_error_m); + latest_stationary_reentry_s = fmax(latest_stationary_reentry_s, + trial.stationary_reentry_time_s); + } + stationary_metrics_t stationary = run_stationary_500hz_scenario(); + check_stationary_metrics(&stationary); + printf("Synthetic 60 s asynchronous ESKF scenario\n"); + printf(" position RMSE horizontal=%6.3f m vertical=%6.3f m\n", + metrics.horizontal_rmse_m, metrics.vertical_rmse_m); + printf(" final error horizontal=%6.3f m vertical=%6.3f m speed=%7.4f m/s\n", + metrics.final_horizontal_error_m, metrics.final_vertical_error_m, + metrics.final_speed_mps); + printf(" motion timing moving=%6.3f s stationary-again=%6.3f s false-still=%u\n", + metrics.moving_detection_time_s, metrics.stationary_reentry_time_s, + metrics.false_stationary_samples); + printf(" GPS/replay accepted=%llu rejected=%llu replay=%llu replayed-imu=%llu\n", + (unsigned long long)metrics.accepted_gps, + (unsigned long long)metrics.rejected_gps, + (unsigned long long)metrics.replay_count, + (unsigned long long)metrics.replayed_imu_count); + printf(" outlier isolation injected=%u rejected=%u max-state-jump=%.3e m\n", + metrics.injected_outliers, metrics.rejected_injected_outliers, + metrics.max_rejected_state_jump_m); + printf(" covariance PSD=%d symmetry-error=%.3e zero-updates=%llu\n", + metrics.covariance_positive_semidefinite ? 1 : 0, + metrics.covariance_symmetry_error, + (unsigned long long)metrics.zero_updates); + printf(" 8-seed robustness worst-h-rmse=%6.3f m worst-final-h=%6.3f m latest-stop=%6.3f s\n", + worst_horizontal_rmse_m, worst_final_horizontal_error_m, + latest_stationary_reentry_s); + printf("Synthetic 30 s stationary / 500 Hz scenario\n"); + printf(" final error horizontal=%6.3f m vertical=%6.3f m speed=%7.4f m/s\n", + stationary.final_horizontal_error_m, + stationary.final_vertical_error_m, + stationary.final_speed_mps); + printf(" decisions false-moving-samples=%u GPS-accepted=%llu zero-updates=%llu\n", + stationary.nonstationary_samples, + (unsigned long long)stationary.accepted_gps, + (unsigned long long)stationary.zero_updates); + if(failures != 0) + { + fprintf(stderr, "%d synthetic fusion check(s) failed\n", failures); + return 1; + } + printf("PASS: production bare-metal ESKF sources passed the Linux/GCC scenario.\n"); + return 0; +} diff --git a/soc/software/gps_system/Makefile b/soc/software/gps_system/Makefile index e39fb31..5f3d306 100644 --- a/soc/software/gps_system/Makefile +++ b/soc/software/gps_system/Makefile @@ -7,6 +7,14 @@ BSP_UART_CLOCK_HZ ?= 50000000 APP_SRCS := \ gps_test.c \ + src/tdk_icm42688.c \ + src/eskf.c \ + src/fusion_motion.c \ + src/fusion_engine.c \ + src/fusion_bus.c \ + src/fusion_gps_adapter.c \ + src/navigation_fusion.c \ + src/gps_fusion_publisher.c \ src/gps_ip.c \ src/gps_interrupt_handlers.c \ src/gps_time_anchor.c \ @@ -15,9 +23,16 @@ APP_SRCS := \ src/NavEquations.c \ src/matrixLib.c +BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c + +TDK_IMU_ODR_HZ ?= 500 +GPS_NAV_SOLVE_TIMER_TICKS ?= 50 + include ../common/app.mk CFLAGS += -Iheader +CFLAGS += -DTDK_IMU_ODR_HZ=$(TDK_IMU_ODR_HZ) \ + -DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS) LDLIBS += -lm ifeq ($(GPS_DEBUG),1) diff --git a/soc/software/gps_system/gps_test.c b/soc/software/gps_system/gps_test.c index 7d694ed..0035d29 100644 --- a/soc/software/gps_system/gps_test.c +++ b/soc/software/gps_system/gps_test.c @@ -5,21 +5,54 @@ #include #include "matrixLib.h" #include "gps_macros.h" +#include "navigation_fusion.h" +#include "tdk_icm42688.h" int main() { - - gps_startup_initialization(15); + tdk_icm42688_config_t imu_config; + tdk_icm42688_default_config(&imu_config); + bool imu_initialized = tdk_icm42688_init(&imu_config); + bool fusion_initialized = navigation_fusion_init(BSP_CLINT_TIMEBASE_HZ); + + gps_startup_initialization(GPS_NAV_SOLVE_TIMER_TICKS); + bool imu_running = imu_initialized && fusion_initialized && tdk_icm42688_start(); + GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION, + "init imu=%u running=%u odr_hz=%u timebase_hz=%lu\r\n", + imu_initialized ? 1u : 0u, imu_running ? 1u : 0u, + imu_config.odr_hz, (unsigned long)BSP_CLINT_TIMEBASE_HZ); acq_start_loop(); + uint64_t last_fusion_status_tick = 0u; while(1) { asm volatile("wfi"); gps_background_task(); calculate_position(); - // TODO: implement calculate position nav solve - //calculate_position_nav_solve(); + if(fusion_initialized) + { + (void)navigation_fusion_service(); + uint64_t now = gps_get_systick(); + if((now - last_fusion_status_tick) >= GPS_SYSTICK_HZ) + { + navigation_fusion_output_t output; + if(navigation_fusion_get_output(&output)) + { + GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION, + "state=%s lat=%.10lf lon=%.10lf height=%.3lf " + "speed=%.3lf nis=%.3lf gps=%lu/%lu sigma=%.2lf,%.2lf,%.2lf\r\n", + fusion_motion_state_name(output.motion_state), + output.latitude_deg, output.longitude_deg, output.height_m, + output.speed_mps, output.last_position_nis, + (unsigned long)output.gps_accepted, + (unsigned long)output.gps_nis_rejected, + output.position_std_m[0], output.position_std_m[1], + output.position_std_m[2]); + } + last_fusion_status_tick = now; + } + } } return 0; diff --git a/soc/software/gps_system/header/eskf.h b/soc/software/gps_system/header/eskf.h new file mode 100644 index 0000000..0579dbd --- /dev/null +++ b/soc/software/gps_system/header/eskf.h @@ -0,0 +1,85 @@ +#ifndef ESKF_H +#define ESKF_H + +#include +#include + +#define ESKF_VECTOR_DIM 3u +#define ESKF_QUATERNION_DIM 4u +#define ESKF_ERROR_STATE_DIM 15u + +typedef enum { + ESKF_UPDATE_INVALID = 0, + ESKF_UPDATE_ACCEPTED, + ESKF_UPDATE_REJECTED_NIS +} eskf_update_result_t; + +typedef struct { + double accel_noise_density[ESKF_VECTOR_DIM]; + double gyro_noise_density[ESKF_VECTOR_DIM]; + double accel_bias_random_walk[ESKF_VECTOR_DIM]; + double gyro_bias_random_walk[ESKF_VECTOR_DIM]; + double initial_position_std_m[ESKF_VECTOR_DIM]; + double initial_velocity_std_mps[ESKF_VECTOR_DIM]; + double initial_attitude_std_rad[ESKF_VECTOR_DIM]; + double initial_accel_bias_std_mps2[ESKF_VECTOR_DIM]; + double initial_gyro_bias_std_rps[ESKF_VECTOR_DIM]; + double gravity_mps2; + double max_propagation_step_s; + double position_nis_gate; + double covariance_diagonal_floor; +} eskf_config_t; + +typedef struct { + double position_m[ESKF_VECTOR_DIM]; + double velocity_mps[ESKF_VECTOR_DIM]; + double body_to_nav_quaternion[ESKF_QUATERNION_DIM]; + double accel_bias_mps2[ESKF_VECTOR_DIM]; + double gyro_bias_rps[ESKF_VECTOR_DIM]; + double covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM]; + double last_linear_accel_nav_mps2[ESKF_VECTOR_DIM]; + double last_unbiased_gyro_rps[ESKF_VECTOR_DIM]; + double last_position_nis; + uint64_t propagation_count; + uint64_t position_update_accepted_count; + uint64_t position_update_rejected_count; + bool initialized; +} eskf_t; + +void eskf_default_config(eskf_config_t* config); + +bool eskf_quaternion_align_vectors(const double source[ESKF_VECTOR_DIM], + const double target[ESKF_VECTOR_DIM], + double quaternion_out[ESKF_QUATERNION_DIM]); + +bool eskf_initialize(eskf_t* filter, + const eskf_config_t* config, + const double initial_position_m[ESKF_VECTOR_DIM], + const double body_to_nav_quaternion[ESKF_QUATERNION_DIM], + const double accel_bias_mps2[ESKF_VECTOR_DIM], + const double gyro_bias_rps[ESKF_VECTOR_DIM]); + +bool eskf_propagate(eskf_t* filter, + const eskf_config_t* config, + const double accel_mps2[ESKF_VECTOR_DIM], + const double gyro_rps[ESKF_VECTOR_DIM], + double dt_s); + +eskf_update_result_t eskf_position_update( + eskf_t* filter, + const eskf_config_t* config, + const double position_m[ESKF_VECTOR_DIM], + const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM], + double* nis_out); + +bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps); + +bool eskf_zero_angular_rate_update( + eskf_t* filter, + const double measured_gyro_rps[ESKF_VECTOR_DIM], + const double measurement_std_rps[ESKF_VECTOR_DIM]); + +double eskf_speed_mps(const eskf_t* filter); +bool eskf_state_is_finite(const eskf_t* filter); + +#endif diff --git a/soc/software/gps_system/header/fusion_bus.h b/soc/software/gps_system/header/fusion_bus.h new file mode 100644 index 0000000..8be0eb3 --- /dev/null +++ b/soc/software/gps_system/header/fusion_bus.h @@ -0,0 +1,42 @@ +#ifndef FUSION_BUS_H +#define FUSION_BUS_H + +#include +#include + +#include "fusion_measurement.h" + +#ifndef FUSION_IMU_QUEUE_CAPACITY +#define FUSION_IMU_QUEUE_CAPACITY 256u +#endif + +#ifndef FUSION_GPS_QUEUE_CAPACITY +#define FUSION_GPS_QUEUE_CAPACITY 8u +#endif + +typedef struct { + fusion_imu_measurement_t imu_queue[FUSION_IMU_QUEUE_CAPACITY]; + fusion_gps_measurement_t gps_queue[FUSION_GPS_QUEUE_CAPACITY]; + volatile unsigned int imu_head; + volatile unsigned int imu_tail; + volatile unsigned int gps_head; + volatile unsigned int gps_tail; + volatile uint64_t imu_overflow_count; + volatile uint64_t gps_overflow_count; +} fusion_bus_t; + +void fusion_bus_init(fusion_bus_t* bus); + +/* Single producer (TDK ISR), single consumer (foreground). */ +bool fusion_bus_publish_imu(fusion_bus_t* bus, + const fusion_imu_measurement_t* measurement); +bool fusion_bus_take_imu(fusion_bus_t* bus, + fusion_imu_measurement_t* measurement_out); + +/* Single producer (GPS WLS foreground), single fusion consumer. */ +bool fusion_bus_publish_gps(fusion_bus_t* bus, + const fusion_gps_measurement_t* measurement); +bool fusion_bus_take_gps(fusion_bus_t* bus, + fusion_gps_measurement_t* measurement_out); + +#endif diff --git a/soc/software/gps_system/header/fusion_engine.h b/soc/software/gps_system/header/fusion_engine.h new file mode 100644 index 0000000..9f35fdf --- /dev/null +++ b/soc/software/gps_system/header/fusion_engine.h @@ -0,0 +1,123 @@ +#ifndef FUSION_ENGINE_H +#define FUSION_ENGINE_H + +#include +#include + +#include "eskf.h" +#include "fusion_measurement.h" +#include "fusion_motion.h" + +#ifndef FUSION_IMU_HISTORY_CAPACITY +#define FUSION_IMU_HISTORY_CAPACITY 2048u +#endif + +#ifndef FUSION_POSITION_PENDING_CAPACITY +#define FUSION_POSITION_PENDING_CAPACITY 8u +#endif + +typedef enum { + FUSION_ENGINE_CALIBRATING = 0, + FUSION_ENGINE_RUNNING, + FUSION_ENGINE_FAULT +} fusion_engine_state_t; + +typedef struct { + eskf_config_t eskf; + fusion_motion_config_t motion; + uint64_t timebase_hz; + double calibration_duration_s; + unsigned int calibration_min_samples; + double calibration_accel_norm_tolerance_mps2; + double calibration_gyro_limit_rps; + double calibration_accel_std_limit_mps2; + double calibration_gyro_std_limit_rps; + double zero_update_period_s; + double zero_velocity_std_mps; + double zero_gyro_std_rps[3]; + double gnss_motion_speed_threshold_mps; + double gnss_displacement_nis_gate; + unsigned int gnss_moving_confirmations; + unsigned int gnss_still_confirmations; +} fusion_engine_config_t; + +typedef struct { + eskf_t filter; + fusion_motion_detector_t motion; + fusion_imu_measurement_t held_imu; + fusion_gnss_motion_evidence_t gnss_evidence; + double last_accepted_position_m[3]; + double last_accepted_position_covariance_m2[9]; + uint64_t filter_timestamp_ticks; + uint64_t last_zero_update_ticks; + uint64_t last_accepted_position_ticks; + unsigned int gnss_moving_count; + unsigned int gnss_still_count; + bool have_held_imu; + bool have_accepted_position; +} fusion_runtime_t; + +typedef struct { + uint64_t imu_received; + uint64_t imu_rejected; + uint64_t imu_history_overflow; + uint64_t checkpoint_advance_count; + uint64_t calibration_reset_count; + uint64_t position_received; + uint64_t position_source_rejected; + uint64_t position_nis_rejected; + uint64_t position_stale_rejected; + uint64_t position_pending_overflow; + uint64_t position_accepted; + uint64_t replay_count; + uint64_t replayed_imu_count; + uint64_t zero_velocity_update_count; +} fusion_engine_diagnostics_t; + +typedef struct { + uint64_t start_ticks; + uint64_t last_ticks; + double accel_mean[3]; + double gyro_mean[3]; + double accel_m2[3]; + double gyro_m2[3]; + unsigned int sample_count; + bool have_start; +} fusion_calibration_accumulator_t; + +typedef struct { + fusion_engine_config_t config; + fusion_engine_state_t state; + fusion_runtime_t current; + fusion_runtime_t checkpoint; + fusion_calibration_accumulator_t calibration; + fusion_imu_measurement_t imu_history[FUSION_IMU_HISTORY_CAPACITY]; + fusion_position_measurement_t pending_positions[FUSION_POSITION_PENDING_CAPACITY]; + fusion_position_measurement_t initial_position; + fusion_engine_diagnostics_t diagnostics; + unsigned int imu_history_head; + unsigned int imu_history_count; + unsigned int pending_position_count; + bool checkpoint_valid; + bool have_initial_position; +} fusion_engine_t; + +void fusion_engine_default_config(fusion_engine_config_t* config, + uint64_t timebase_hz); +bool fusion_engine_init(fusion_engine_t* engine, + const fusion_engine_config_t* config); + +bool fusion_engine_push_imu(fusion_engine_t* engine, + const fusion_imu_measurement_t* measurement); +bool fusion_engine_push_position( + fusion_engine_t* engine, + const fusion_position_measurement_t* measurement, + eskf_update_result_t* update_result_out, + double* nis_out); + +bool fusion_engine_get_runtime(const fusion_engine_t* engine, + fusion_runtime_t* runtime_out); +const fusion_engine_diagnostics_t* fusion_engine_diagnostics( + const fusion_engine_t* engine); + +#endif diff --git a/soc/software/gps_system/header/fusion_gps_adapter.h b/soc/software/gps_system/header/fusion_gps_adapter.h new file mode 100644 index 0000000..7d4c994 --- /dev/null +++ b/soc/software/gps_system/header/fusion_gps_adapter.h @@ -0,0 +1,37 @@ +#ifndef FUSION_GPS_ADAPTER_H +#define FUSION_GPS_ADAPTER_H + +#include + +#include "fusion_measurement.h" + +typedef struct { + double minimum_horizontal_std_m; + double minimum_vertical_std_m; + double maximum_position_std_m; +} fusion_gps_adapter_config_t; + +typedef struct { + double origin_latitude_deg; + double origin_longitude_deg; + double origin_height_m; + double origin_ecef_m[3]; + double ecef_to_enu[3][3]; + fusion_gps_adapter_config_t config; + bool origin_valid; +} fusion_gps_adapter_t; + +void fusion_gps_adapter_default_config(fusion_gps_adapter_config_t* config); +bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter, + const fusion_gps_adapter_config_t* config); +bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter, + const fusion_gps_measurement_t* gps, + fusion_position_measurement_t* position_out); +bool fusion_gps_adapter_local_to_geodetic( + const fusion_gps_adapter_t* adapter, + const double position_enu_m[3], + double* latitude_deg_out, + double* longitude_deg_out, + double* height_m_out); + +#endif diff --git a/soc/software/gps_system/header/fusion_measurement.h b/soc/software/gps_system/header/fusion_measurement.h new file mode 100644 index 0000000..2fc2132 --- /dev/null +++ b/soc/software/gps_system/header/fusion_measurement.h @@ -0,0 +1,43 @@ +#ifndef FUSION_MEASUREMENT_H +#define FUSION_MEASUREMENT_H + +#include +#include + +/* + * Hardware- and solver-neutral messages shared at the GPS/ESKF boundary. + * timestamp_ticks always belongs to the common local monotonic timebase; GPS + * week/TOW is metadata and must not drive filter dt. + */ +typedef struct { + uint64_t timestamp_ticks; + double latitude_deg; + double longitude_deg; + double height_m; + double covariance_enu_m2[3][3]; + double hacc_m; + double vacc_m; + double residual_rms_m; + double chi_square_ratio; + unsigned int used_satellites; + bool solution_valid; +} fusion_gps_measurement_t; + +typedef struct { + uint64_t timestamp_ticks; + double accel_mps2[3]; + double gyro_rps[3]; + double temperature_c; + uint32_t sequence; +} fusion_imu_measurement_t; + +/* Generic local-navigation measurement consumed by the ESKF coordinator. */ +typedef struct { + uint64_t timestamp_ticks; + double position_enu_m[3]; + double covariance_enu_m2[9]; + double source_quality; + bool source_valid; +} fusion_position_measurement_t; + +#endif diff --git a/soc/software/gps_system/header/fusion_motion.h b/soc/software/gps_system/header/fusion_motion.h new file mode 100644 index 0000000..0feb294 --- /dev/null +++ b/soc/software/gps_system/header/fusion_motion.h @@ -0,0 +1,63 @@ +#ifndef FUSION_MOTION_H +#define FUSION_MOTION_H + +#include +#include + +typedef enum { + FUSION_MOTION_BOOTSTRAP = 0, + FUSION_MOTION_STATIONARY, + FUSION_MOTION_MOVING, + FUSION_MOTION_STATIONARY_CANDIDATE +} fusion_motion_state_t; + +typedef enum { + FUSION_GNSS_MOTION_UNKNOWN = 0, + FUSION_GNSS_MOTION_STILL, + FUSION_GNSS_MOTION_MOVING +} fusion_gnss_motion_evidence_t; + +typedef struct { + double stationary_accel_limit_mps2; + double stationary_gyro_limit_rps; + double moving_accel_threshold_mps2; + double moving_gyro_threshold_rps; + double moving_jerk_threshold_mps3; + double jerk_filter_time_constant_s; + double stationary_speed_limit_mps; + double moving_speed_threshold_mps; + double stationary_dwell_s; + unsigned int weak_motion_samples; +} fusion_motion_config_t; + +typedef struct { + fusion_motion_state_t state; + double quiet_time_s; + double filtered_linear_accel_mps2[3]; + double previous_filtered_linear_accel_mps2[3]; + double last_linear_accel_norm_mps2; + double last_gyro_norm_rps; + double last_jerk_norm_mps3; + double last_speed_mps; + unsigned int weak_motion_count; + uint64_t transition_count; + bool have_previous_accel; +} fusion_motion_detector_t; + +void fusion_motion_default_config(fusion_motion_config_t* config); +void fusion_motion_init(fusion_motion_detector_t* detector, + bool bootstrap_stationary); + +fusion_motion_state_t fusion_motion_update( + fusion_motion_detector_t* detector, + const fusion_motion_config_t* config, + const double linear_accel_nav_mps2[3], + const double unbiased_gyro_rps[3], + double speed_mps, + fusion_gnss_motion_evidence_t gnss_evidence, + double dt_s); + +bool fusion_motion_allows_zero_update(const fusion_motion_detector_t* detector); +const char* fusion_motion_state_name(fusion_motion_state_t state); + +#endif diff --git a/soc/software/gps_system/header/gps_fusion_publisher.h b/soc/software/gps_system/header/gps_fusion_publisher.h new file mode 100644 index 0000000..d259da3 --- /dev/null +++ b/soc/software/gps_system/header/gps_fusion_publisher.h @@ -0,0 +1,12 @@ +#ifndef GPS_FUSION_PUBLISHER_H +#define GPS_FUSION_PUBLISHER_H + +#include +#include + +#include "NavEquations.h" + +bool gps_fusion_publish_solution(const gps_nav_solution_t* solution, + uint64_t measurement_timestamp_ticks); + +#endif diff --git a/soc/software/gps_system/header/gps_ip_defines.h b/soc/software/gps_system/header/gps_ip_defines.h index ba54447..5920627 100644 --- a/soc/software/gps_system/header/gps_ip_defines.h +++ b/soc/software/gps_system/header/gps_ip_defines.h @@ -46,7 +46,9 @@ #define GPS_EVENT_QUEUE_SIZE 64u #define GPS_SYSTICK_HZ 20u #define GPS_NAV_SOLVE_TIMER_TICK_MS 20u + #ifndef GPS_NAV_SOLVE_TIMER_TICKS #define GPS_NAV_SOLVE_TIMER_TICKS 50u + #endif #define GPS_NAV_SOLVE_PERIOD_MS (GPS_NAV_SOLVE_TIMER_TICKS * GPS_NAV_SOLVE_TIMER_TICK_MS) /* Keep integrity statistics active, but do not let leave-one-out FDE * replace the full-WLS position until the real measurement-noise model @@ -62,7 +64,7 @@ #define GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC 0.001 #define GPS_HOW_ANCHOR_MAX_AGE_SEC 12.0 - _Static_assert(GPS_NAV_SOLVE_PERIOD_MS == 1000u, "NAV solve period must be 1 second"); + _Static_assert(GPS_NAV_SOLVE_TIMER_TICKS > 0u, "NAV solve timer must be non-zero"); _Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u), "GPS_NAV_ENABLE_FDE must be zero or one"); diff --git a/soc/software/gps_system/header/gps_macros.h b/soc/software/gps_system/header/gps_macros.h index dfd1e95..bf80c40 100644 --- a/soc/software/gps_system/header/gps_macros.h +++ b/soc/software/gps_system/header/gps_macros.h @@ -30,6 +30,7 @@ #define GPS_LOG_CAT_SCHED "SCHED" #define GPS_LOG_CAT_MEM "MEM" #define GPS_LOG_CAT_HWREG "HWREG" + #define GPS_LOG_CAT_FUSION "FUSION" // Set the current verbosity level #ifdef DEBUG diff --git a/soc/software/gps_system/header/linked_list.h b/soc/software/gps_system/header/linked_list.h index 2c58e56..7a31520 100644 --- a/soc/software/gps_system/header/linked_list.h +++ b/soc/software/gps_system/header/linked_list.h @@ -17,6 +17,7 @@ typedef struct gps_ephem_ll { volatile uint32_t gps_nav_solve_consumed_sequence; volatile uint32_t gps_last_nav_solve_abs_sample; volatile double gps_last_nav_solve_corrected_abs_sample; + volatile uint64_t gps_last_nav_solve_local_ticks; volatile uint64_t gps_last_nav_solve_pps_counter; volatile uint32_t gps_last_pps_capture_sequence; volatile uint64_t gps_last_measurement_tick; diff --git a/soc/software/gps_system/header/navigation_fusion.h b/soc/software/gps_system/header/navigation_fusion.h new file mode 100644 index 0000000..dd6ec6f --- /dev/null +++ b/soc/software/gps_system/header/navigation_fusion.h @@ -0,0 +1,33 @@ +#ifndef NAVIGATION_FUSION_H +#define NAVIGATION_FUSION_H + +#include +#include + +#include "fusion_engine.h" +#include "fusion_measurement.h" +#include "fusion_motion.h" + +typedef struct { + uint64_t timestamp_ticks; + double position_enu_m[3]; + double velocity_enu_mps[3]; + double latitude_deg; + double longitude_deg; + double height_m; + double position_std_m[3]; + double speed_mps; + double last_position_nis; + fusion_motion_state_t motion_state; + uint64_t gps_accepted; + uint64_t gps_nis_rejected; + bool valid; +} navigation_fusion_output_t; + +bool navigation_fusion_init(uint64_t local_timebase_hz); +bool navigation_fusion_publish_gps(const fusion_gps_measurement_t* measurement); +unsigned int navigation_fusion_service(void); +bool navigation_fusion_get_output(navigation_fusion_output_t* output); +const fusion_engine_t* navigation_fusion_engine(void); + +#endif diff --git a/soc/software/gps_system/header/tdk_icm42688.h b/soc/software/gps_system/header/tdk_icm42688.h new file mode 100644 index 0000000..d4bf592 --- /dev/null +++ b/soc/software/gps_system/header/tdk_icm42688.h @@ -0,0 +1,75 @@ +#ifndef TDK_ICM42688_H +#define TDK_ICM42688_H + +#include +#include + +#include "fusion_measurement.h" + +#ifndef TDK_IMU_RAW_QUEUE_CAPACITY +#define TDK_IMU_RAW_QUEUE_CAPACITY 256u +#endif + +#ifndef TDK_IMU_ODR_HZ +#define TDK_IMU_ODR_HZ 500u +#endif + +typedef enum { + TDK_ACCEL_FS_2G = 2, + TDK_ACCEL_FS_4G = 4, + TDK_ACCEL_FS_8G = 8, + TDK_ACCEL_FS_16G = 16 +} tdk_accel_full_scale_t; + +typedef enum { + TDK_GYRO_FS_15D625 = 15625, + TDK_GYRO_FS_31D25 = 31250, + TDK_GYRO_FS_62D5 = 62500, + TDK_GYRO_FS_125 = 125000, + TDK_GYRO_FS_250 = 250000, + TDK_GYRO_FS_500 = 500000, + TDK_GYRO_FS_1000 = 1000000, + TDK_GYRO_FS_2000 = 2000000 +} tdk_gyro_full_scale_mdps_t; + +typedef struct { + uint32_t odr_hz; + tdk_accel_full_scale_t accel_full_scale_g; + tdk_gyro_full_scale_mdps_t gyro_full_scale_mdps; + uint8_t ui_filter_config; +} tdk_icm42688_config_t; + +typedef struct { + uint64_t timestamp_ticks; + int16_t temperature_raw; + int16_t accel_raw[3]; + int16_t gyro_raw[3]; + uint32_t sequence; +} tdk_icm42688_raw_sample_t; + +typedef struct { + uint64_t irq_count; + uint64_t sample_count; + uint64_t invalid_sample_count; + uint64_t spurious_irq_count; + uint64_t queue_overflow_count; + uint64_t estimated_missed_sample_count; + uint64_t last_timestamp_ticks; + uint8_t who_am_i; + bool initialized; + bool running; +} tdk_icm42688_diagnostics_t; + +void tdk_icm42688_default_config(tdk_icm42688_config_t* config); +bool tdk_icm42688_init(const tdk_icm42688_config_t* config); +bool tdk_icm42688_start(void); +void tdk_icm42688_stop(void); + +/* Called only by the common PLIC ISR for BSP_PMOD_IRQ. */ +void tdk_icm42688_irq_handler(void); + +/* Foreground conversion; no floating-point work is performed in the ISR. */ +bool tdk_icm42688_take_measurement(fusion_imu_measurement_t* measurement_out); +void tdk_icm42688_get_diagnostics(tdk_icm42688_diagnostics_t* diagnostics_out); + +#endif diff --git a/soc/software/gps_system/src/eskf.c b/soc/software/gps_system/src/eskf.c new file mode 100644 index 0000000..969dfcf --- /dev/null +++ b/soc/software/gps_system/src/eskf.c @@ -0,0 +1,809 @@ +#include "eskf.h" + +#include +#include +#include +#include + +#define ESKF_POSITION_INDEX 0u +#define ESKF_VELOCITY_INDEX 3u +#define ESKF_ATTITUDE_INDEX 6u +#define ESKF_ACCEL_BIAS_INDEX 9u +#define ESKF_GYRO_BIAS_INDEX 12u + +#define ESKF_PI 3.14159265358979323846264338327950288 + +static double square(double value) +{ + return value * value; +} + +static double vector_norm3(const double value[3]) +{ + return sqrt(square(value[0]) + square(value[1]) + square(value[2])); +} + +static bool vector_is_finite(const double* value, size_t count) +{ + if(value == NULL) + { + return false; + } + for(size_t i = 0 ; i < count ; i++) + { + if(!isfinite(value[i])) + { + return false; + } + } + return true; +} + +static bool quaternion_normalize(double quaternion[4]) +{ + double norm = sqrt(square(quaternion[0]) + square(quaternion[1]) + + square(quaternion[2]) + square(quaternion[3])); + if(!(norm > DBL_EPSILON) || !isfinite(norm)) + { + return false; + } + for(unsigned int i = 0 ; i < 4u ; i++) + { + quaternion[i] /= norm; + } + return true; +} + +static void quaternion_multiply(const double first[4], const double second[4], + double output[4]) +{ + output[0] = first[0] * second[0] - first[1] * second[1] - + first[2] * second[2] - first[3] * second[3]; + output[1] = first[0] * second[1] + first[1] * second[0] + + first[2] * second[3] - first[3] * second[2]; + output[2] = first[0] * second[2] - first[1] * second[3] + + first[2] * second[0] + first[3] * second[1]; + output[3] = first[0] * second[3] + first[1] * second[2] - + first[2] * second[1] + first[3] * second[0]; +} + +static void rotation_vector_quaternion(const double rotation_vector[3], + double quaternion[4]) +{ + double angle = vector_norm3(rotation_vector); + if(angle < 1.0e-10) + { + quaternion[0] = 1.0; + quaternion[1] = 0.5 * rotation_vector[0]; + quaternion[2] = 0.5 * rotation_vector[1]; + quaternion[3] = 0.5 * rotation_vector[2]; + (void)quaternion_normalize(quaternion); + return; + } + + double scale = sin(0.5 * angle) / angle; + quaternion[0] = cos(0.5 * angle); + quaternion[1] = scale * rotation_vector[0]; + quaternion[2] = scale * rotation_vector[1]; + quaternion[3] = scale * rotation_vector[2]; +} + +static void quaternion_rotation_matrix(const double quaternion[4], + double rotation[3][3]) +{ + const double w = quaternion[0]; + const double x = quaternion[1]; + const double y = quaternion[2]; + const double z = quaternion[3]; + + rotation[0][0] = 1.0 - 2.0 * (y * y + z * z); + rotation[0][1] = 2.0 * (x * y - w * z); + rotation[0][2] = 2.0 * (x * z + w * y); + rotation[1][0] = 2.0 * (x * y + w * z); + rotation[1][1] = 1.0 - 2.0 * (x * x + z * z); + rotation[1][2] = 2.0 * (y * z - w * x); + rotation[2][0] = 2.0 * (x * z - w * y); + rotation[2][1] = 2.0 * (y * z + w * x); + rotation[2][2] = 1.0 - 2.0 * (x * x + y * y); +} + +static void skew_matrix(const double vector[3], double skew[3][3]) +{ + skew[0][0] = 0.0; + skew[0][1] = -vector[2]; + skew[0][2] = vector[1]; + skew[1][0] = vector[2]; + skew[1][1] = 0.0; + skew[1][2] = -vector[0]; + skew[2][0] = -vector[1]; + skew[2][1] = vector[0]; + skew[2][2] = 0.0; +} + +static bool inverse_symmetric3(const double input[9], double inverse[9]) +{ + double lower[3][3] = {{0.0}}; + double scale = fmax(fabs(input[0]), fmax(fabs(input[4]), fabs(input[8]))); + if(!(scale > 0.0) || !isfinite(scale)) + { + return false; + } + + for(unsigned int row = 0 ; row < 3u ; row++) + { + for(unsigned int col = 0 ; col <= row ; col++) + { + double value = 0.5 * (input[row * 3u + col] + input[col * 3u + row]); + for(unsigned int k = 0 ; k < col ; k++) + { + value -= lower[row][k] * lower[col][k]; + } + if(row == col) + { + if(value <= scale * DBL_EPSILON * 128.0 || !isfinite(value)) + { + return false; + } + lower[row][col] = sqrt(value); + } + else + { + lower[row][col] = value / lower[col][col]; + } + } + } + + for(unsigned int column = 0 ; column < 3u ; column++) + { + double forward[3] = {0.0, 0.0, 0.0}; + double solution[3] = {0.0, 0.0, 0.0}; + for(unsigned int row = 0 ; row < 3u ; row++) + { + double rhs = (row == column) ? 1.0 : 0.0; + for(unsigned int k = 0 ; k < row ; k++) + { + rhs -= lower[row][k] * forward[k]; + } + forward[row] = rhs / lower[row][row]; + } + for(int row = 2 ; row >= 0 ; row--) + { + double rhs = forward[row]; + for(unsigned int k = (unsigned int)row + 1u ; k < 3u ; k++) + { + rhs -= lower[k][row] * solution[k]; + } + solution[row] = rhs / lower[row][row]; + } + for(unsigned int row = 0 ; row < 3u ; row++) + { + inverse[row * 3u + column] = solution[row]; + } + } + return vector_is_finite(inverse, 9u); +} + +static void covariance_symmetrize_and_floor(eskf_t* filter, double floor_value) +{ + for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = row + 1u ; col < ESKF_ERROR_STATE_DIM ; col++) + { + double symmetric = 0.5 * + (filter->covariance[row][col] + filter->covariance[col][row]); + filter->covariance[row][col] = symmetric; + filter->covariance[col][row] = symmetric; + } + if(filter->covariance[row][row] < floor_value) + { + filter->covariance[row][row] = floor_value; + } + } +} + +void eskf_default_config(eskf_config_t* config) +{ + if(config == NULL) + { + return; + } + memset(config, 0, sizeof(*config)); + for(unsigned int i = 0 ; i < 3u ; i++) + { + config->accel_noise_density[i] = 0.03; + config->gyro_noise_density[i] = 0.003; + config->accel_bias_random_walk[i] = 0.002; + config->gyro_bias_random_walk[i] = 0.002 * ESKF_PI / 180.0; + config->initial_position_std_m[i] = (i < 2u) ? 10.0 : 20.0; + config->initial_velocity_std_mps[i] = 1.0; + config->initial_attitude_std_rad[i] = + ((i < 2u) ? 5.0 : 30.0) * ESKF_PI / 180.0; + config->initial_accel_bias_std_mps2[i] = 0.1; + config->initial_gyro_bias_std_rps[i] = 0.05 * ESKF_PI / 180.0; + } + config->gravity_mps2 = 9.80665; + config->max_propagation_step_s = 0.02; + config->position_nis_gate = 16.26623619623813; /* chi-square 3 DOF, 99.9%. */ + config->covariance_diagonal_floor = 1.0e-12; +} + +bool eskf_quaternion_align_vectors(const double source[3], + const double target[3], + double quaternion_out[4]) +{ + if(!vector_is_finite(source, 3u) || !vector_is_finite(target, 3u) || + (quaternion_out == NULL)) + { + return false; + } + double source_norm = vector_norm3(source); + double target_norm = vector_norm3(target); + if(!(source_norm > DBL_EPSILON) || !(target_norm > DBL_EPSILON)) + { + return false; + } + double first[3]; + double second[3]; + for(unsigned int i = 0 ; i < 3u ; i++) + { + first[i] = source[i] / source_norm; + second[i] = target[i] / target_norm; + } + double dot = first[0] * second[0] + first[1] * second[1] + first[2] * second[2]; + if(dot < -0.999999) + { + double axis[3] = {0.0, first[2], -first[1]}; + if(vector_norm3(axis) < 1.0e-6) + { + axis[0] = -first[2]; + axis[1] = 0.0; + axis[2] = first[0]; + } + double axis_norm = vector_norm3(axis); + quaternion_out[0] = 0.0; + for(unsigned int i = 0 ; i < 3u ; i++) + { + quaternion_out[i + 1u] = axis[i] / axis_norm; + } + return true; + } + + quaternion_out[0] = 1.0 + dot; + quaternion_out[1] = first[1] * second[2] - first[2] * second[1]; + quaternion_out[2] = first[2] * second[0] - first[0] * second[2]; + quaternion_out[3] = first[0] * second[1] - first[1] * second[0]; + return quaternion_normalize(quaternion_out); +} + +bool eskf_initialize(eskf_t* filter, + const eskf_config_t* config, + const double initial_position_m[3], + const double body_to_nav_quaternion[4], + const double accel_bias_mps2[3], + const double gyro_bias_rps[3]) +{ + if((filter == NULL) || (config == NULL) || + !vector_is_finite(initial_position_m, 3u) || + !vector_is_finite(body_to_nav_quaternion, 4u) || + !vector_is_finite(accel_bias_mps2, 3u) || + !vector_is_finite(gyro_bias_rps, 3u) || + !(config->gravity_mps2 > 0.0)) + { + return false; + } + + memset(filter, 0, sizeof(*filter)); + memcpy(filter->position_m, initial_position_m, sizeof(filter->position_m)); + memcpy(filter->body_to_nav_quaternion, body_to_nav_quaternion, + sizeof(filter->body_to_nav_quaternion)); + memcpy(filter->accel_bias_mps2, accel_bias_mps2, + sizeof(filter->accel_bias_mps2)); + memcpy(filter->gyro_bias_rps, gyro_bias_rps, sizeof(filter->gyro_bias_rps)); + if(!quaternion_normalize(filter->body_to_nav_quaternion)) + { + return false; + } + + for(unsigned int i = 0 ; i < 3u ; i++) + { + filter->covariance[ESKF_POSITION_INDEX + i][ESKF_POSITION_INDEX + i] = + square(config->initial_position_std_m[i]); + filter->covariance[ESKF_VELOCITY_INDEX + i][ESKF_VELOCITY_INDEX + i] = + square(config->initial_velocity_std_mps[i]); + filter->covariance[ESKF_ATTITUDE_INDEX + i][ESKF_ATTITUDE_INDEX + i] = + square(config->initial_attitude_std_rad[i]); + filter->covariance[ESKF_ACCEL_BIAS_INDEX + i][ESKF_ACCEL_BIAS_INDEX + i] = + square(config->initial_accel_bias_std_mps2[i]); + filter->covariance[ESKF_GYRO_BIAS_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] = + square(config->initial_gyro_bias_std_rps[i]); + } + covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor); + filter->last_position_nis = NAN; + filter->initialized = true; + return true; +} + +static bool eskf_propagate_step(eskf_t* filter, + const eskf_config_t* config, + const double accel_mps2[3], + const double gyro_rps[3], + double dt_s) +{ + double specific_force[3]; + double angular_rate[3]; + for(unsigned int i = 0 ; i < 3u ; i++) + { + specific_force[i] = accel_mps2[i] - filter->accel_bias_mps2[i]; + angular_rate[i] = gyro_rps[i] - filter->gyro_bias_rps[i]; + filter->last_unbiased_gyro_rps[i] = angular_rate[i]; + } + + double rotation[3][3]; + quaternion_rotation_matrix(filter->body_to_nav_quaternion, rotation); + double linear_accel[3] = {0.0, 0.0, -config->gravity_mps2}; + for(unsigned int row = 0 ; row < 3u ; row++) + { + for(unsigned int col = 0 ; col < 3u ; col++) + { + linear_accel[row] += rotation[row][col] * specific_force[col]; + } + filter->last_linear_accel_nav_mps2[row] = linear_accel[row]; + filter->position_m[row] += filter->velocity_mps[row] * dt_s + + 0.5 * linear_accel[row] * dt_s * dt_s; + filter->velocity_mps[row] += linear_accel[row] * dt_s; + } + + double rotation_vector[3] = { + angular_rate[0] * dt_s, + angular_rate[1] * dt_s, + angular_rate[2] * dt_s + }; + double delta_quaternion[4]; + double updated_quaternion[4]; + rotation_vector_quaternion(rotation_vector, delta_quaternion); + quaternion_multiply(filter->body_to_nav_quaternion, delta_quaternion, + updated_quaternion); + memcpy(filter->body_to_nav_quaternion, updated_quaternion, + sizeof(updated_quaternion)); + if(!quaternion_normalize(filter->body_to_nav_quaternion)) + { + return false; + } + + double system[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}}; + for(unsigned int i = 0 ; i < 3u ; i++) + { + system[ESKF_POSITION_INDEX + i][ESKF_VELOCITY_INDEX + i] = 1.0; + system[ESKF_ATTITUDE_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] = -1.0; + } + double force_skew[3][3]; + double rate_skew[3][3]; + skew_matrix(specific_force, force_skew); + skew_matrix(angular_rate, rate_skew); + for(unsigned int row = 0 ; row < 3u ; row++) + { + for(unsigned int col = 0 ; col < 3u ; col++) + { + double rotated_skew = 0.0; + for(unsigned int k = 0 ; k < 3u ; k++) + { + rotated_skew += rotation[row][k] * force_skew[k][col]; + } + system[ESKF_VELOCITY_INDEX + row][ESKF_ATTITUDE_INDEX + col] = + -rotated_skew; + system[ESKF_VELOCITY_INDEX + row][ESKF_ACCEL_BIAS_INDEX + col] = + -rotation[row][col]; + system[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] = + -rate_skew[row][col]; + } + } + + double transition[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}}; + for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++) + { + transition[row][col] = system[row][col] * dt_s; + } + transition[row][row] += 1.0; + } + + double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}}; + double propagated[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}}; + for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++) + { + for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++) + { + intermediate[row][col] += + transition[row][k] * filter->covariance[k][col]; + } + } + } + for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++) + { + for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++) + { + propagated[row][col] += intermediate[row][k] * transition[col][k]; + } + } + } + + for(unsigned int row = 0 ; row < 3u ; row++) + { + for(unsigned int col = 0 ; col < 3u ; col++) + { + double accel_noise = 0.0; + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + accel_noise += rotation[row][axis] * + square(config->accel_noise_density[axis]) * + rotation[col][axis]; + } + propagated[ESKF_VELOCITY_INDEX + row][ESKF_VELOCITY_INDEX + col] += + accel_noise * dt_s; + } + propagated[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + row] += + square(config->gyro_noise_density[row]) * dt_s; + propagated[ESKF_ACCEL_BIAS_INDEX + row][ESKF_ACCEL_BIAS_INDEX + row] += + square(config->accel_bias_random_walk[row]) * dt_s; + propagated[ESKF_GYRO_BIAS_INDEX + row][ESKF_GYRO_BIAS_INDEX + row] += + square(config->gyro_bias_random_walk[row]) * dt_s; + } + memcpy(filter->covariance, propagated, sizeof(propagated)); + covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor); + filter->propagation_count++; + return eskf_state_is_finite(filter); +} + +bool eskf_propagate(eskf_t* filter, + const eskf_config_t* config, + const double accel_mps2[3], + const double gyro_rps[3], + double dt_s) +{ + if((filter == NULL) || !filter->initialized || (config == NULL) || + !vector_is_finite(accel_mps2, 3u) || !vector_is_finite(gyro_rps, 3u) || + !isfinite(dt_s) || (dt_s < 0.0) || + !(config->max_propagation_step_s > 0.0)) + { + return false; + } + if(dt_s == 0.0) + { + return true; + } + unsigned int steps = (unsigned int)ceil(dt_s / config->max_propagation_step_s); + if((steps == 0u) || (steps > 10000u)) + { + return false; + } + double step_s = dt_s / (double)steps; + for(unsigned int step = 0 ; step < steps ; step++) + { + if(!eskf_propagate_step(filter, config, accel_mps2, gyro_rps, step_s)) + { + return false; + } + } + return true; +} + +static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STATE_DIM]) +{ + for(unsigned int i = 0 ; i < 3u ; i++) + { + filter->position_m[i] += error[ESKF_POSITION_INDEX + i]; + filter->velocity_mps[i] += error[ESKF_VELOCITY_INDEX + i]; + filter->accel_bias_mps2[i] += error[ESKF_ACCEL_BIAS_INDEX + i]; + filter->gyro_bias_rps[i] += error[ESKF_GYRO_BIAS_INDEX + i]; + } + double attitude_error[3] = { + error[ESKF_ATTITUDE_INDEX], + error[ESKF_ATTITUDE_INDEX + 1u], + error[ESKF_ATTITUDE_INDEX + 2u] + }; + double delta_quaternion[4]; + double updated_quaternion[4]; + rotation_vector_quaternion(attitude_error, delta_quaternion); + quaternion_multiply(filter->body_to_nav_quaternion, delta_quaternion, + updated_quaternion); + memcpy(filter->body_to_nav_quaternion, updated_quaternion, + sizeof(updated_quaternion)); + if(!quaternion_normalize(filter->body_to_nav_quaternion)) + { + return false; + } + + /* Right attitude-error reset after q <- q (x) Exp(delta_theta). */ + double reset[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}}; + for(unsigned int i = 0 ; i < ESKF_ERROR_STATE_DIM ; i++) + { + reset[i][i] = 1.0; + } + double attitude_skew[3][3]; + skew_matrix(attitude_error, attitude_skew); + for(unsigned int row = 0 ; row < 3u ; row++) + { + for(unsigned int col = 0 ; col < 3u ; col++) + { + reset[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] -= + 0.5 * attitude_skew[row][col]; + } + } + double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}}; + double reset_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}}; + for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++) + { + for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++) + { + intermediate[row][col] += reset[row][k] * filter->covariance[k][col]; + } + } + } + for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++) + { + for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++) + { + reset_covariance[row][col] += intermediate[row][k] * reset[col][k]; + } + } + } + memcpy(filter->covariance, reset_covariance, sizeof(reset_covariance)); + return true; +} + +static bool eskf_selected_update(eskf_t* filter, + unsigned int selected_state_index, + const double residual[3], + const double measurement_covariance[9], + bool gate_enabled, + double gate, + double* nis_out) +{ + if((selected_state_index + 3u) > ESKF_ERROR_STATE_DIM || + !vector_is_finite(residual, 3u) || + !vector_is_finite(measurement_covariance, 9u)) + { + return false; + } + double covariance_inverse[9]; + if(!inverse_symmetric3(measurement_covariance, covariance_inverse)) + { + return false; + } + double innovation[9]; + for(unsigned int row = 0 ; row < 3u ; row++) + { + for(unsigned int col = 0 ; col < 3u ; col++) + { + innovation[row * 3u + col] = + filter->covariance[selected_state_index + row][selected_state_index + col] + + measurement_covariance[row * 3u + col]; + } + } + double innovation_inverse[9]; + if(!inverse_symmetric3(innovation, innovation_inverse)) + { + return false; + } + double nis = 0.0; + for(unsigned int row = 0 ; row < 3u ; row++) + { + for(unsigned int col = 0 ; col < 3u ; col++) + { + nis += residual[row] * innovation_inverse[row * 3u + col] * residual[col]; + } + } + if(nis_out != NULL) + { + *nis_out = nis; + } + if(!isfinite(nis) || (gate_enabled && (nis > gate))) + { + return false; + } + + double gain[ESKF_ERROR_STATE_DIM][3] = {{0.0}}; + for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = 0 ; col < 3u ; col++) + { + for(unsigned int k = 0 ; k < 3u ; k++) + { + gain[row][col] += + filter->covariance[row][selected_state_index + k] * + innovation_inverse[k * 3u + col]; + } + } + } + double error[ESKF_ERROR_STATE_DIM] = {0.0}; + for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = 0 ; col < 3u ; col++) + { + error[row] += gain[row][col] * residual[col]; + } + } + + double joseph_left[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}}; + for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++) + { + joseph_left[row][row] = 1.0; + for(unsigned int col = 0 ; col < 3u ; col++) + { + joseph_left[row][selected_state_index + col] -= gain[row][col]; + } + } + double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}}; + double updated_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}}; + for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++) + { + for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++) + { + intermediate[row][col] += + joseph_left[row][k] * filter->covariance[k][col]; + } + } + } + for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++) + { + for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++) + { + for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++) + { + updated_covariance[row][col] += + intermediate[row][k] * joseph_left[col][k]; + } + for(unsigned int first = 0 ; first < 3u ; first++) + { + for(unsigned int second = 0 ; second < 3u ; second++) + { + updated_covariance[row][col] += gain[row][first] * + measurement_covariance[first * 3u + second] * gain[col][second]; + } + } + } + } + memcpy(filter->covariance, updated_covariance, sizeof(updated_covariance)); + if(!eskf_inject_error(filter, error)) + { + return false; + } + covariance_symmetrize_and_floor(filter, 1.0e-12); + return eskf_state_is_finite(filter); +} + +eskf_update_result_t eskf_position_update( + eskf_t* filter, + const eskf_config_t* config, + const double position_m[3], + const double covariance_m2[9], + double* nis_out) +{ + if(nis_out != NULL) + { + *nis_out = NAN; + } + if((filter == NULL) || !filter->initialized || (config == NULL) || + !vector_is_finite(position_m, 3u) || !vector_is_finite(covariance_m2, 9u)) + { + return ESKF_UPDATE_INVALID; + } + double residual[3]; + for(unsigned int i = 0 ; i < 3u ; i++) + { + residual[i] = position_m[i] - filter->position_m[i]; + } + double nis = NAN; + if(!eskf_selected_update(filter, ESKF_POSITION_INDEX, residual, covariance_m2, + true, config->position_nis_gate, &nis)) + { + filter->last_position_nis = nis; + if(isfinite(nis) && (nis > config->position_nis_gate)) + { + filter->position_update_rejected_count++; + if(nis_out != NULL) + { + *nis_out = nis; + } + return ESKF_UPDATE_REJECTED_NIS; + } + return ESKF_UPDATE_INVALID; + } + filter->last_position_nis = nis; + filter->position_update_accepted_count++; + if(nis_out != NULL) + { + *nis_out = nis; + } + return ESKF_UPDATE_ACCEPTED; +} + +bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps) +{ + if((filter == NULL) || !filter->initialized || + !(velocity_std_mps > 0.0) || !isfinite(velocity_std_mps)) + { + return false; + } + double residual[3] = { + -filter->velocity_mps[0], + -filter->velocity_mps[1], + -filter->velocity_mps[2] + }; + double covariance[9] = {0.0}; + for(unsigned int i = 0 ; i < 3u ; i++) + { + covariance[i * 3u + i] = square(velocity_std_mps); + } + return eskf_selected_update(filter, ESKF_VELOCITY_INDEX, residual, covariance, + false, 0.0, NULL); +} + +bool eskf_zero_angular_rate_update( + eskf_t* filter, + const double measured_gyro_rps[3], + const double measurement_std_rps[3]) +{ + if((filter == NULL) || !filter->initialized || + !vector_is_finite(measured_gyro_rps, 3u) || + !vector_is_finite(measurement_std_rps, 3u)) + { + return false; + } + double residual[3]; + double covariance[9] = {0.0}; + for(unsigned int i = 0 ; i < 3u ; i++) + { + if(!(measurement_std_rps[i] > 0.0)) + { + return false; + } + residual[i] = measured_gyro_rps[i] - filter->gyro_bias_rps[i]; + covariance[i * 3u + i] = square(measurement_std_rps[i]); + } + return eskf_selected_update(filter, ESKF_GYRO_BIAS_INDEX, residual, covariance, + false, 0.0, NULL); +} + +double eskf_speed_mps(const eskf_t* filter) +{ + if((filter == NULL) || !filter->initialized) + { + return NAN; + } + return vector_norm3(filter->velocity_mps); +} + +bool eskf_state_is_finite(const eskf_t* filter) +{ + if((filter == NULL) || !filter->initialized || + !vector_is_finite(filter->position_m, 3u) || + !vector_is_finite(filter->velocity_mps, 3u) || + !vector_is_finite(filter->body_to_nav_quaternion, 4u) || + !vector_is_finite(filter->accel_bias_mps2, 3u) || + !vector_is_finite(filter->gyro_bias_rps, 3u) || + !vector_is_finite(&filter->covariance[0][0], + ESKF_ERROR_STATE_DIM * ESKF_ERROR_STATE_DIM)) + { + return false; + } + for(unsigned int i = 0 ; i < ESKF_ERROR_STATE_DIM ; i++) + { + if(filter->covariance[i][i] < 0.0) + { + return false; + } + } + return true; +} diff --git a/soc/software/gps_system/src/fusion_bus.c b/soc/software/gps_system/src/fusion_bus.c new file mode 100644 index 0000000..8a2f5b1 --- /dev/null +++ b/soc/software/gps_system/src/fusion_bus.c @@ -0,0 +1,102 @@ +#include "fusion_bus.h" + +#include +#include + +#if (FUSION_IMU_QUEUE_CAPACITY < 2u) +#error "FUSION_IMU_QUEUE_CAPACITY must be at least 2" +#endif + +#if (FUSION_GPS_QUEUE_CAPACITY < 2u) +#error "FUSION_GPS_QUEUE_CAPACITY must be at least 2" +#endif + +static unsigned int next_index(unsigned int index, unsigned int capacity) +{ + index++; + return (index == capacity) ? 0u : index; +} + +void fusion_bus_init(fusion_bus_t* bus) +{ + if(bus != NULL) + { + memset(bus, 0, sizeof(*bus)); + } +} + +bool fusion_bus_publish_imu(fusion_bus_t* bus, + const fusion_imu_measurement_t* measurement) +{ + if((bus == NULL) || (measurement == NULL)) + { + return false; + } + unsigned int head = __atomic_load_n(&bus->imu_head, __ATOMIC_RELAXED); + unsigned int next = next_index(head, FUSION_IMU_QUEUE_CAPACITY); + if(next == __atomic_load_n(&bus->imu_tail, __ATOMIC_ACQUIRE)) + { + __atomic_fetch_add(&bus->imu_overflow_count, 1u, __ATOMIC_RELAXED); + return false; + } + bus->imu_queue[head] = *measurement; + __atomic_store_n(&bus->imu_head, next, __ATOMIC_RELEASE); + return true; +} + +bool fusion_bus_take_imu(fusion_bus_t* bus, + fusion_imu_measurement_t* measurement_out) +{ + if((bus == NULL) || (measurement_out == NULL)) + { + return false; + } + unsigned int tail = __atomic_load_n(&bus->imu_tail, __ATOMIC_RELAXED); + if(tail == __atomic_load_n(&bus->imu_head, __ATOMIC_ACQUIRE)) + { + return false; + } + *measurement_out = bus->imu_queue[tail]; + __atomic_store_n(&bus->imu_tail, + next_index(tail, FUSION_IMU_QUEUE_CAPACITY), + __ATOMIC_RELEASE); + return true; +} + +bool fusion_bus_publish_gps(fusion_bus_t* bus, + const fusion_gps_measurement_t* measurement) +{ + if((bus == NULL) || (measurement == NULL)) + { + return false; + } + unsigned int head = __atomic_load_n(&bus->gps_head, __ATOMIC_RELAXED); + unsigned int next = next_index(head, FUSION_GPS_QUEUE_CAPACITY); + if(next == __atomic_load_n(&bus->gps_tail, __ATOMIC_ACQUIRE)) + { + __atomic_fetch_add(&bus->gps_overflow_count, 1u, __ATOMIC_RELAXED); + return false; + } + bus->gps_queue[head] = *measurement; + __atomic_store_n(&bus->gps_head, next, __ATOMIC_RELEASE); + return true; +} + +bool fusion_bus_take_gps(fusion_bus_t* bus, + fusion_gps_measurement_t* measurement_out) +{ + if((bus == NULL) || (measurement_out == NULL)) + { + return false; + } + unsigned int tail = __atomic_load_n(&bus->gps_tail, __ATOMIC_RELAXED); + if(tail == __atomic_load_n(&bus->gps_head, __ATOMIC_ACQUIRE)) + { + return false; + } + *measurement_out = bus->gps_queue[tail]; + __atomic_store_n(&bus->gps_tail, + next_index(tail, FUSION_GPS_QUEUE_CAPACITY), + __ATOMIC_RELEASE); + return true; +} diff --git a/soc/software/gps_system/src/fusion_engine.c b/soc/software/gps_system/src/fusion_engine.c new file mode 100644 index 0000000..534f476 --- /dev/null +++ b/soc/software/gps_system/src/fusion_engine.c @@ -0,0 +1,730 @@ +#include "fusion_engine.h" + +#include +#include +#include +#include + +#define FUSION_PI 3.14159265358979323846264338327950288 + +static double square(double value) +{ + return value * value; +} + +static double norm3(const double value[3]) +{ + return sqrt(square(value[0]) + square(value[1]) + square(value[2])); +} + +static bool finite_values(const double* values, unsigned int count) +{ + if(values == NULL) + { + return false; + } + for(unsigned int i = 0 ; i < count ; i++) + { + if(!isfinite(values[i])) + { + return false; + } + } + return true; +} + +static double ticks_to_seconds(const fusion_engine_t* engine, uint64_t ticks) +{ + return (double)ticks / (double)engine->config.timebase_hz; +} + +static uint64_t seconds_to_ticks(const fusion_engine_t* engine, double seconds) +{ + double ticks = seconds * (double)engine->config.timebase_hz; + if(ticks <= 1.0) + { + return 1u; + } + return (uint64_t)(ticks + 0.5); +} + +void fusion_engine_default_config(fusion_engine_config_t* config, + uint64_t timebase_hz) +{ + if(config == NULL) + { + return; + } + memset(config, 0, sizeof(*config)); + eskf_default_config(&config->eskf); + fusion_motion_default_config(&config->motion); + config->timebase_hz = timebase_hz; + config->calibration_duration_s = 2.0; + config->calibration_min_samples = 50u; + config->calibration_accel_norm_tolerance_mps2 = 0.30; + config->calibration_gyro_limit_rps = 2.0 * FUSION_PI / 180.0; + config->calibration_accel_std_limit_mps2 = 0.12; + config->calibration_gyro_std_limit_rps = 0.15 * FUSION_PI / 180.0; + config->zero_update_period_s = 0.05; + config->zero_velocity_std_mps = 0.03; + for(unsigned int i = 0 ; i < 3u ; i++) + { + config->zero_gyro_std_rps[i] = 0.01 * FUSION_PI / 180.0; + } + config->gnss_motion_speed_threshold_mps = 0.50; + config->gnss_displacement_nis_gate = 9.21034037197618; /* 2 DOF, 99%. */ + config->gnss_moving_confirmations = 2u; + config->gnss_still_confirmations = 2u; +} + +static bool config_valid(const fusion_engine_config_t* config) +{ + return (config != NULL) && (config->timebase_hz != 0u) && + isfinite(config->calibration_duration_s) && + (config->calibration_duration_s > 0.0) && + (config->calibration_min_samples >= 2u) && + isfinite(config->calibration_accel_norm_tolerance_mps2) && + (config->calibration_accel_norm_tolerance_mps2 > 0.0) && + isfinite(config->calibration_gyro_limit_rps) && + (config->calibration_gyro_limit_rps > 0.0) && + isfinite(config->zero_update_period_s) && + (config->zero_update_period_s > 0.0) && + isfinite(config->zero_velocity_std_mps) && + (config->zero_velocity_std_mps > 0.0) && + finite_values(config->zero_gyro_std_rps, 3u) && + (config->gnss_moving_confirmations != 0u) && + (config->gnss_still_confirmations != 0u); +} + +bool fusion_engine_init(fusion_engine_t* engine, + const fusion_engine_config_t* config) +{ + if((engine == NULL) || !config_valid(config)) + { + return false; + } + memset(engine, 0, sizeof(*engine)); + engine->config = *config; + engine->state = FUSION_ENGINE_CALIBRATING; + return true; +} + +static bool imu_valid(const fusion_imu_measurement_t* measurement) +{ + return (measurement != NULL) && + finite_values(measurement->accel_mps2, 3u) && + finite_values(measurement->gyro_rps, 3u) && + isfinite(measurement->temperature_c); +} + +static bool position_valid(const fusion_position_measurement_t* measurement) +{ + return (measurement != NULL) && + finite_values(measurement->position_enu_m, 3u) && + finite_values(measurement->covariance_enu_m2, 9u) && + isfinite(measurement->source_quality); +} + +static void calibration_reset(fusion_engine_t* engine) +{ + memset(&engine->calibration, 0, sizeof(engine->calibration)); + engine->diagnostics.calibration_reset_count++; +} + +static void calibration_add(fusion_calibration_accumulator_t* calibration, + const fusion_imu_measurement_t* measurement) +{ + calibration->sample_count++; + double count = (double)calibration->sample_count; + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + double accel_delta = measurement->accel_mps2[axis] - calibration->accel_mean[axis]; + calibration->accel_mean[axis] += accel_delta / count; + calibration->accel_m2[axis] += + accel_delta * (measurement->accel_mps2[axis] - calibration->accel_mean[axis]); + double gyro_delta = measurement->gyro_rps[axis] - calibration->gyro_mean[axis]; + calibration->gyro_mean[axis] += gyro_delta / count; + calibration->gyro_m2[axis] += + gyro_delta * (measurement->gyro_rps[axis] - calibration->gyro_mean[axis]); + } +} + +static bool calibration_sample_is_quiet(const fusion_engine_t* engine, + const fusion_imu_measurement_t* measurement) +{ + double accel_error = fabs(norm3(measurement->accel_mps2) - engine->config.eskf.gravity_mps2); + return (accel_error <= engine->config.calibration_accel_norm_tolerance_mps2) && + (norm3(measurement->gyro_rps) <= engine->config.calibration_gyro_limit_rps); +} + +static bool calibration_statistics_are_quiet(const fusion_engine_t* engine) +{ + const fusion_calibration_accumulator_t* calibration = &engine->calibration; + if(calibration->sample_count < 2u) + { + return false; + } + double denominator = (double)(calibration->sample_count - 1u); + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + double accel_std = sqrt(fmax(0.0, calibration->accel_m2[axis] / denominator)); + double gyro_std = sqrt(fmax(0.0, calibration->gyro_m2[axis] / denominator)); + if((accel_std > engine->config.calibration_accel_std_limit_mps2) || + (gyro_std > engine->config.calibration_gyro_std_limit_rps)) + { + return false; + } + } + return true; +} + +static void history_clear(fusion_engine_t* engine) +{ + engine->imu_history_head = 0u; + engine->imu_history_count = 0u; +} + +static const fusion_imu_measurement_t* history_at(const fusion_engine_t* engine, + unsigned int offset) +{ + unsigned int index = (engine->imu_history_head + offset) % FUSION_IMU_HISTORY_CAPACITY; + return &engine->imu_history[index]; +} + +static bool runtime_apply_imu(fusion_engine_t* engine, + fusion_runtime_t* runtime, + const fusion_imu_measurement_t* measurement, + bool count_zero_updates); + +static void history_push(fusion_engine_t* engine, + const fusion_imu_measurement_t* measurement) +{ + if(engine->imu_history_count == FUSION_IMU_HISTORY_CAPACITY) + { + const fusion_imu_measurement_t oldest = *history_at(engine, 0u); + if(engine->checkpoint_valid && + (oldest.timestamp_ticks > engine->checkpoint.filter_timestamp_ticks)) + { + if(!runtime_apply_imu(engine, &engine->checkpoint, &oldest, false)) + { + engine->state = FUSION_ENGINE_FAULT; + return; + } + engine->diagnostics.checkpoint_advance_count++; + } + engine->imu_history_head = + (engine->imu_history_head + 1u) % FUSION_IMU_HISTORY_CAPACITY; + engine->imu_history_count--; + engine->diagnostics.imu_history_overflow++; + } + unsigned int index = (engine->imu_history_head + engine->imu_history_count) % + FUSION_IMU_HISTORY_CAPACITY; + engine->imu_history[index] = *measurement; + engine->imu_history_count++; +} + +static void history_drop_through(fusion_engine_t* engine, uint64_t timestamp_ticks) +{ + while((engine->imu_history_count != 0u) && + (history_at(engine, 0u)->timestamp_ticks <= timestamp_ticks)) + { + engine->imu_history_head = + (engine->imu_history_head + 1u) % FUSION_IMU_HISTORY_CAPACITY; + engine->imu_history_count--; + } +} + +static bool runtime_propagate_to(fusion_engine_t* engine, + fusion_runtime_t* runtime, + uint64_t timestamp_ticks, + const fusion_imu_measurement_t* input) +{ + if((runtime == NULL) || (input == NULL) || !runtime->filter.initialized || + (timestamp_ticks < runtime->filter_timestamp_ticks)) + { + return false; + } + if(timestamp_ticks == runtime->filter_timestamp_ticks) + { + return true; + } + double dt_s = ticks_to_seconds(engine, + timestamp_ticks - runtime->filter_timestamp_ticks); + if(!eskf_propagate(&runtime->filter, &engine->config.eskf, + input->accel_mps2, input->gyro_rps, dt_s)) + { + return false; + } + runtime->filter_timestamp_ticks = timestamp_ticks; + return true; +} + +static bool runtime_apply_imu(fusion_engine_t* engine, + fusion_runtime_t* runtime, + const fusion_imu_measurement_t* measurement, + bool count_zero_updates) +{ + if(!runtime->have_held_imu) + { + runtime->held_imu = *measurement; + runtime->have_held_imu = true; + runtime->filter_timestamp_ticks = measurement->timestamp_ticks; + return true; + } + if((measurement->timestamp_ticks <= runtime->held_imu.timestamp_ticks) || + (measurement->timestamp_ticks < runtime->filter_timestamp_ticks)) + { + return false; + } + double motion_dt_s = ticks_to_seconds( + engine, measurement->timestamp_ticks - runtime->held_imu.timestamp_ticks); + if(!runtime_propagate_to(engine, runtime, measurement->timestamp_ticks, measurement)) + { + return false; + } + (void)fusion_motion_update( + &runtime->motion, &engine->config.motion, + runtime->filter.last_linear_accel_nav_mps2, + runtime->filter.last_unbiased_gyro_rps, + eskf_speed_mps(&runtime->filter), runtime->gnss_evidence, motion_dt_s); + bool inertial_motion = + (runtime->motion.last_linear_accel_norm_mps2 >= + engine->config.motion.moving_accel_threshold_mps2) || + (runtime->motion.last_gyro_norm_rps >= + engine->config.motion.moving_gyro_threshold_rps) || + (runtime->motion.last_jerk_norm_mps3 >= + engine->config.motion.moving_jerk_threshold_mps3); + if(inertial_motion && + (runtime->gnss_evidence == FUSION_GNSS_MOTION_STILL)) + { + /* A new IMU motion onset makes an older GNSS-still decision stale. */ + runtime->gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN; + runtime->gnss_moving_count = 0u; + runtime->gnss_still_count = 0u; + } + runtime->held_imu = *measurement; + + uint64_t zero_period_ticks = + seconds_to_ticks(engine, engine->config.zero_update_period_s); + if(fusion_motion_allows_zero_update(&runtime->motion) && + ((measurement->timestamp_ticks - runtime->last_zero_update_ticks) >= + zero_period_ticks)) + { + if(!eskf_zero_velocity_update(&runtime->filter, + engine->config.zero_velocity_std_mps) || + !eskf_zero_angular_rate_update(&runtime->filter, + measurement->gyro_rps, + engine->config.zero_gyro_std_rps)) + { + return false; + } + runtime->last_zero_update_ticks = measurement->timestamp_ticks; + if(count_zero_updates) + { + engine->diagnostics.zero_velocity_update_count++; + } + } + return true; +} + +static bool finish_calibration(fusion_engine_t* engine, + const fusion_imu_measurement_t* last_measurement) +{ + if(!calibration_statistics_are_quiet(engine)) + { + calibration_reset(engine); + return false; + } + const fusion_calibration_accumulator_t* calibration = &engine->calibration; + double gravity_norm = norm3(calibration->accel_mean); + if(!(gravity_norm > DBL_EPSILON)) + { + calibration_reset(engine); + return false; + } + double nav_up[3] = {0.0, 0.0, 1.0}; + double quaternion[4]; + if(!eskf_quaternion_align_vectors(calibration->accel_mean, nav_up, quaternion)) + { + engine->state = FUSION_ENGINE_FAULT; + return false; + } + double accel_bias[3]; + double gyro_bias[3]; + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + double expected_gravity = calibration->accel_mean[axis] * + engine->config.eskf.gravity_mps2 / gravity_norm; + accel_bias[axis] = calibration->accel_mean[axis] - expected_gravity; + gyro_bias[axis] = calibration->gyro_mean[axis]; + } + double initial_position[3] = {0.0, 0.0, 0.0}; + if(engine->have_initial_position) + { + memcpy(initial_position, engine->initial_position.position_enu_m, + sizeof(initial_position)); + } + if(!eskf_initialize(&engine->current.filter, &engine->config.eskf, + initial_position, quaternion, accel_bias, gyro_bias)) + { + engine->state = FUSION_ENGINE_FAULT; + return false; + } + fusion_motion_init(&engine->current.motion, true); + engine->current.held_imu = *last_measurement; + engine->current.have_held_imu = true; + engine->current.filter_timestamp_ticks = last_measurement->timestamp_ticks; + engine->current.last_zero_update_ticks = last_measurement->timestamp_ticks; + engine->current.gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN; + if(engine->have_initial_position) + { + engine->current.have_accepted_position = true; + engine->current.last_accepted_position_ticks = + engine->initial_position.timestamp_ticks; + memcpy(engine->current.last_accepted_position_m, + engine->initial_position.position_enu_m, + sizeof(engine->current.last_accepted_position_m)); + memcpy(engine->current.last_accepted_position_covariance_m2, + engine->initial_position.covariance_enu_m2, + sizeof(engine->current.last_accepted_position_covariance_m2)); + } + engine->checkpoint = engine->current; + engine->checkpoint_valid = true; + history_clear(engine); + engine->state = FUSION_ENGINE_RUNNING; + return true; +} + +static bool process_calibration_imu(fusion_engine_t* engine, + const fusion_imu_measurement_t* measurement) +{ + if(engine->calibration.have_start && + (measurement->timestamp_ticks <= engine->calibration.last_ticks)) + { + return false; + } + if(!calibration_sample_is_quiet(engine, measurement)) + { + calibration_reset(engine); + return true; + } + if(!engine->calibration.have_start) + { + engine->calibration.start_ticks = measurement->timestamp_ticks; + engine->calibration.have_start = true; + } + engine->calibration.last_ticks = measurement->timestamp_ticks; + calibration_add(&engine->calibration, measurement); + double duration_s = ticks_to_seconds( + engine, engine->calibration.last_ticks - engine->calibration.start_ticks); + if((engine->calibration.sample_count >= engine->config.calibration_min_samples) && + (duration_s >= engine->config.calibration_duration_s)) + { + (void)finish_calibration(engine, measurement); + } + return true; +} + +static fusion_gnss_motion_evidence_t position_motion_evidence( + const fusion_engine_t* engine, + const fusion_runtime_t* runtime, + const fusion_position_measurement_t* measurement) +{ + if(!runtime->have_accepted_position || + (measurement->timestamp_ticks <= runtime->last_accepted_position_ticks)) + { + return FUSION_GNSS_MOTION_UNKNOWN; + } + double dt_s = ticks_to_seconds( + engine, measurement->timestamp_ticks - runtime->last_accepted_position_ticks); + if(!(dt_s > 0.0)) + { + return FUSION_GNSS_MOTION_UNKNOWN; + } + double delta_e = measurement->position_enu_m[0] - runtime->last_accepted_position_m[0]; + double delta_n = measurement->position_enu_m[1] - runtime->last_accepted_position_m[1]; + double s00 = measurement->covariance_enu_m2[0] + + runtime->last_accepted_position_covariance_m2[0]; + double s01 = measurement->covariance_enu_m2[1] + + runtime->last_accepted_position_covariance_m2[1]; + double s10 = measurement->covariance_enu_m2[3] + + runtime->last_accepted_position_covariance_m2[3]; + double s11 = measurement->covariance_enu_m2[4] + + runtime->last_accepted_position_covariance_m2[4]; + double off_diagonal = 0.5 * (s01 + s10); + double determinant = s00 * s11 - off_diagonal * off_diagonal; + if(!(s00 > 0.0) || !(s11 > 0.0) || !(determinant > DBL_EPSILON) || + !isfinite(determinant)) + { + return FUSION_GNSS_MOTION_UNKNOWN; + } + double displacement_nis = + (delta_e * delta_e * s11 - 2.0 * delta_e * delta_n * off_diagonal + + delta_n * delta_n * s00) / determinant; + double speed_mps = sqrt(delta_e * delta_e + delta_n * delta_n) / dt_s; + if((speed_mps >= engine->config.gnss_motion_speed_threshold_mps) && + (displacement_nis >= engine->config.gnss_displacement_nis_gate)) + { + return FUSION_GNSS_MOTION_MOVING; + } + return FUSION_GNSS_MOTION_STILL; +} + +static eskf_update_result_t runtime_apply_position( + fusion_engine_t* engine, + fusion_runtime_t* runtime, + const fusion_position_measurement_t* measurement, + double* nis_out) +{ + eskf_update_result_t result = eskf_position_update( + &runtime->filter, &engine->config.eskf, + measurement->position_enu_m, measurement->covariance_enu_m2, nis_out); + if(result == ESKF_UPDATE_ACCEPTED) + { + fusion_gnss_motion_evidence_t raw_evidence = + position_motion_evidence(engine, runtime, measurement); + if(raw_evidence == FUSION_GNSS_MOTION_MOVING) + { + runtime->gnss_still_count = 0u; + if(runtime->gnss_moving_count < UINT32_MAX) + { + runtime->gnss_moving_count++; + } + if(runtime->gnss_moving_count >= engine->config.gnss_moving_confirmations) + { + runtime->gnss_evidence = FUSION_GNSS_MOTION_MOVING; + } + } + else if(raw_evidence == FUSION_GNSS_MOTION_STILL) + { + runtime->gnss_moving_count = 0u; + if(runtime->gnss_still_count < UINT32_MAX) + { + runtime->gnss_still_count++; + } + if(runtime->gnss_still_count >= engine->config.gnss_still_confirmations) + { + runtime->gnss_evidence = FUSION_GNSS_MOTION_STILL; + } + } + runtime->have_accepted_position = true; + runtime->last_accepted_position_ticks = measurement->timestamp_ticks; + memcpy(runtime->last_accepted_position_m, measurement->position_enu_m, + sizeof(runtime->last_accepted_position_m)); + memcpy(runtime->last_accepted_position_covariance_m2, + measurement->covariance_enu_m2, + sizeof(runtime->last_accepted_position_covariance_m2)); + } + return result; +} + +static bool process_position_now(fusion_engine_t* engine, + const fusion_position_measurement_t* measurement, + eskf_update_result_t* result_out, + double* nis_out) +{ + if(!engine->checkpoint_valid || + (measurement->timestamp_ticks < engine->checkpoint.filter_timestamp_ticks)) + { + engine->diagnostics.position_stale_rejected++; + return false; + } + fusion_runtime_t revised = engine->checkpoint; + uint64_t replayed = 0u; + for(unsigned int i = 0 ; i < engine->imu_history_count ; i++) + { + const fusion_imu_measurement_t* imu = history_at(engine, i); + if(imu->timestamp_ticks > measurement->timestamp_ticks) + { + break; + } + if(!runtime_apply_imu(engine, &revised, imu, false)) + { + engine->state = FUSION_ENGINE_FAULT; + return false; + } + replayed++; + } + if(revised.filter_timestamp_ticks < measurement->timestamp_ticks) + { + if(!revised.have_held_imu || + !runtime_propagate_to(engine, &revised, + measurement->timestamp_ticks, &revised.held_imu)) + { + engine->state = FUSION_ENGINE_FAULT; + return false; + } + } + double nis = NAN; + eskf_update_result_t result = runtime_apply_position(engine, &revised, + measurement, &nis); + if(result == ESKF_UPDATE_INVALID) + { + return false; + } + if(result == ESKF_UPDATE_ACCEPTED) + { + engine->diagnostics.position_accepted++; + } + else + { + engine->diagnostics.position_nis_rejected++; + } + engine->checkpoint = revised; + engine->checkpoint_valid = true; + history_drop_through(engine, measurement->timestamp_ticks); + for(unsigned int i = 0 ; i < engine->imu_history_count ; i++) + { + if(!runtime_apply_imu(engine, &revised, history_at(engine, i), false)) + { + engine->state = FUSION_ENGINE_FAULT; + return false; + } + replayed++; + } + engine->current = revised; + engine->diagnostics.replay_count++; + engine->diagnostics.replayed_imu_count += replayed; + if(result_out != NULL) + { + *result_out = result; + } + if(nis_out != NULL) + { + *nis_out = nis; + } + return true; +} + +static bool pending_insert(fusion_engine_t* engine, + const fusion_position_measurement_t* measurement) +{ + if(engine->pending_position_count >= FUSION_POSITION_PENDING_CAPACITY) + { + engine->diagnostics.position_pending_overflow++; + return false; + } + unsigned int index = engine->pending_position_count; + while((index != 0u) && + (engine->pending_positions[index - 1u].timestamp_ticks > + measurement->timestamp_ticks)) + { + engine->pending_positions[index] = engine->pending_positions[index - 1u]; + index--; + } + engine->pending_positions[index] = *measurement; + engine->pending_position_count++; + return true; +} + +static void process_ready_pending_positions(fusion_engine_t* engine) +{ + while((engine->pending_position_count != 0u) && + (engine->pending_positions[0].timestamp_ticks <= + engine->current.filter_timestamp_ticks)) + { + fusion_position_measurement_t measurement = engine->pending_positions[0]; + for(unsigned int i = 1u ; i < engine->pending_position_count ; i++) + { + engine->pending_positions[i - 1u] = engine->pending_positions[i]; + } + engine->pending_position_count--; + (void)process_position_now(engine, &measurement, NULL, NULL); + if(engine->state == FUSION_ENGINE_FAULT) + { + return; + } + } +} + +bool fusion_engine_push_imu(fusion_engine_t* engine, + const fusion_imu_measurement_t* measurement) +{ + if((engine == NULL) || (engine->state == FUSION_ENGINE_FAULT) || + !imu_valid(measurement)) + { + if(engine != NULL) + { + engine->diagnostics.imu_rejected++; + } + return false; + } + engine->diagnostics.imu_received++; + if(engine->state == FUSION_ENGINE_CALIBRATING) + { + bool ok = process_calibration_imu(engine, measurement); + if(!ok) + { + engine->diagnostics.imu_rejected++; + } + return ok; + } + if(!runtime_apply_imu(engine, &engine->current, measurement, true)) + { + engine->diagnostics.imu_rejected++; + return false; + } + history_push(engine, measurement); + if(engine->state == FUSION_ENGINE_FAULT) + { + return false; + } + process_ready_pending_positions(engine); + return engine->state == FUSION_ENGINE_RUNNING; +} + +bool fusion_engine_push_position( + fusion_engine_t* engine, + const fusion_position_measurement_t* measurement, + eskf_update_result_t* update_result_out, + double* nis_out) +{ + if(update_result_out != NULL) + { + *update_result_out = ESKF_UPDATE_INVALID; + } + if(nis_out != NULL) + { + *nis_out = NAN; + } + if((engine == NULL) || (engine->state == FUSION_ENGINE_FAULT) || + !position_valid(measurement)) + { + return false; + } + engine->diagnostics.position_received++; + if(!measurement->source_valid) + { + engine->diagnostics.position_source_rejected++; + return false; + } + if(engine->state == FUSION_ENGINE_CALIBRATING) + { + engine->initial_position = *measurement; + engine->have_initial_position = true; + return true; + } + if(measurement->timestamp_ticks > engine->current.filter_timestamp_ticks) + { + return pending_insert(engine, measurement); + } + return process_position_now(engine, measurement, update_result_out, nis_out); +} + +bool fusion_engine_get_runtime(const fusion_engine_t* engine, + fusion_runtime_t* runtime_out) +{ + if((engine == NULL) || (runtime_out == NULL) || + (engine->state != FUSION_ENGINE_RUNNING)) + { + return false; + } + *runtime_out = engine->current; + return true; +} + +const fusion_engine_diagnostics_t* fusion_engine_diagnostics( + const fusion_engine_t* engine) +{ + return (engine != NULL) ? &engine->diagnostics : NULL; +} diff --git a/soc/software/gps_system/src/fusion_gps_adapter.c b/soc/software/gps_system/src/fusion_gps_adapter.c new file mode 100644 index 0000000..6cc57fb --- /dev/null +++ b/soc/software/gps_system/src/fusion_gps_adapter.c @@ -0,0 +1,254 @@ +#include "fusion_gps_adapter.h" + +#include +#include +#include +#include + +#define WGS84_SEMI_MAJOR_M 6378137.0 +#define WGS84_FLATTENING (1.0 / 298.257223563) +#define FUSION_GPS_PI 3.14159265358979323846264338327950288 + +static double radians(double degrees) +{ + return degrees * (FUSION_GPS_PI / 180.0); +} + +static double degrees(double radians_value) +{ + return radians_value * (180.0 / FUSION_GPS_PI); +} + +static bool llh_valid(double latitude_deg, double longitude_deg, double height_m) +{ + return isfinite(latitude_deg) && isfinite(longitude_deg) && isfinite(height_m) && + (latitude_deg >= -90.0) && (latitude_deg <= 90.0) && + (longitude_deg >= -180.0) && (longitude_deg <= 180.0) && + (height_m >= -10000.0) && (height_m <= 100000.0); +} + +static void geodetic_to_ecef(double latitude_deg, double longitude_deg, + double height_m, double ecef_m[3]) +{ + double latitude = radians(latitude_deg); + double longitude = radians(longitude_deg); + double flattening = WGS84_FLATTENING; + double eccentricity_squared = flattening * (2.0 - flattening); + double sin_latitude = sin(latitude); + double cos_latitude = cos(latitude); + double prime_vertical = WGS84_SEMI_MAJOR_M / + sqrt(1.0 - eccentricity_squared * + sin_latitude * sin_latitude); + ecef_m[0] = (prime_vertical + height_m) * cos_latitude * cos(longitude); + ecef_m[1] = (prime_vertical + height_m) * cos_latitude * sin(longitude); + ecef_m[2] = (prime_vertical * (1.0 - eccentricity_squared) + height_m) * + sin_latitude; +} + +static void configure_origin(fusion_gps_adapter_t* adapter, + const fusion_gps_measurement_t* gps) +{ + adapter->origin_latitude_deg = gps->latitude_deg; + adapter->origin_longitude_deg = gps->longitude_deg; + adapter->origin_height_m = gps->height_m; + geodetic_to_ecef(gps->latitude_deg, gps->longitude_deg, gps->height_m, + adapter->origin_ecef_m); + double latitude = radians(gps->latitude_deg); + double longitude = radians(gps->longitude_deg); + double sin_latitude = sin(latitude); + double cos_latitude = cos(latitude); + double sin_longitude = sin(longitude); + double cos_longitude = cos(longitude); + adapter->ecef_to_enu[0][0] = -sin_longitude; + adapter->ecef_to_enu[0][1] = cos_longitude; + adapter->ecef_to_enu[0][2] = 0.0; + adapter->ecef_to_enu[1][0] = -sin_latitude * cos_longitude; + adapter->ecef_to_enu[1][1] = -sin_latitude * sin_longitude; + adapter->ecef_to_enu[1][2] = cos_latitude; + adapter->ecef_to_enu[2][0] = cos_latitude * cos_longitude; + adapter->ecef_to_enu[2][1] = cos_latitude * sin_longitude; + adapter->ecef_to_enu[2][2] = sin_latitude; + adapter->origin_valid = true; +} + +void fusion_gps_adapter_default_config(fusion_gps_adapter_config_t* config) +{ + if(config == NULL) + { + return; + } + config->minimum_horizontal_std_m = 0.50; + config->minimum_vertical_std_m = 1.00; + config->maximum_position_std_m = 1000.0; +} + +bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter, + const fusion_gps_adapter_config_t* config) +{ + if((adapter == NULL) || (config == NULL) || + !(config->minimum_horizontal_std_m > 0.0) || + !(config->minimum_vertical_std_m > 0.0) || + !(config->maximum_position_std_m >= config->minimum_vertical_std_m)) + { + return false; + } + memset(adapter, 0, sizeof(*adapter)); + adapter->config = *config; + return true; +} + +static bool sanitize_covariance(const fusion_gps_adapter_t* adapter, + const double input[3][3], double output[9]) +{ + double variance_floor[3] = { + adapter->config.minimum_horizontal_std_m * + adapter->config.minimum_horizontal_std_m, + adapter->config.minimum_horizontal_std_m * + adapter->config.minimum_horizontal_std_m, + adapter->config.minimum_vertical_std_m * + adapter->config.minimum_vertical_std_m + }; + double variance_ceiling = adapter->config.maximum_position_std_m * + adapter->config.maximum_position_std_m; + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + double variance = input[axis][axis]; + if(!isfinite(variance) || (variance <= 0.0)) + { + return false; + } + output[axis * 3u + axis] = fmin(fmax(variance, variance_floor[axis]), + variance_ceiling); + } + for(unsigned int row = 0 ; row < 3u ; row++) + { + for(unsigned int col = row + 1u ; col < 3u ; col++) + { + double covariance = 0.5 * (input[row][col] + input[col][row]); + if(!isfinite(covariance)) + { + return false; + } + double limit = 0.95 * sqrt(output[row * 3u + row] * + output[col * 3u + col]); + covariance = fmin(fmax(covariance, -limit), limit); + output[row * 3u + col] = covariance; + output[col * 3u + row] = covariance; + } + } + /* Sylvester criterion: the sanitized symmetric covariance must be PD. */ + double minor2 = output[0] * output[4] - output[1] * output[3]; + double determinant = + output[0] * (output[4] * output[8] - output[5] * output[7]) - + output[1] * (output[3] * output[8] - output[5] * output[6]) + + output[2] * (output[3] * output[7] - output[4] * output[6]); + return (output[0] > 0.0) && (minor2 > DBL_EPSILON) && + (determinant > DBL_EPSILON) && isfinite(determinant); +} + +bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter, + const fusion_gps_measurement_t* gps, + fusion_position_measurement_t* position_out) +{ + if((adapter == NULL) || (gps == NULL) || (position_out == NULL) || + !llh_valid(gps->latitude_deg, gps->longitude_deg, gps->height_m)) + { + return false; + } + memset(position_out, 0, sizeof(*position_out)); + position_out->timestamp_ticks = gps->timestamp_ticks; + position_out->source_valid = gps->solution_valid; + position_out->source_quality = + (isfinite(gps->chi_square_ratio) && (gps->chi_square_ratio >= 0.0)) + ? 100.0 / (1.0 + gps->chi_square_ratio) : 0.0; + if(!sanitize_covariance(adapter, gps->covariance_enu_m2, + position_out->covariance_enu_m2)) + { + return false; + } + if(!adapter->origin_valid) + { + if(!gps->solution_valid) + { + return false; + } + configure_origin(adapter, gps); + } + double ecef_m[3]; + double delta_ecef_m[3]; + geodetic_to_ecef(gps->latitude_deg, gps->longitude_deg, gps->height_m, ecef_m); + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + delta_ecef_m[axis] = ecef_m[axis] - adapter->origin_ecef_m[axis]; + } + for(unsigned int row = 0 ; row < 3u ; row++) + { + for(unsigned int col = 0 ; col < 3u ; col++) + { + position_out->position_enu_m[row] += + adapter->ecef_to_enu[row][col] * delta_ecef_m[col]; + } + } + return isfinite(position_out->position_enu_m[0]) && + isfinite(position_out->position_enu_m[1]) && + isfinite(position_out->position_enu_m[2]); +} + +bool fusion_gps_adapter_local_to_geodetic( + const fusion_gps_adapter_t* adapter, + const double position_enu_m[3], + double* latitude_deg_out, + double* longitude_deg_out, + double* height_m_out) +{ + if((adapter == NULL) || !adapter->origin_valid || (position_enu_m == NULL) || + (latitude_deg_out == NULL) || (longitude_deg_out == NULL) || + (height_m_out == NULL)) + { + return false; + } + double ecef_m[3]; + for(unsigned int row = 0 ; row < 3u ; row++) + { + ecef_m[row] = adapter->origin_ecef_m[row]; + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + /* R^-1 = R^T for the orthonormal ECEF-to-ENU transform. */ + ecef_m[row] += adapter->ecef_to_enu[axis][row] * position_enu_m[axis]; + } + } + double flattening = WGS84_FLATTENING; + double eccentricity_squared = flattening * (2.0 - flattening); + double longitude = atan2(ecef_m[1], ecef_m[0]); + double horizontal = hypot(ecef_m[0], ecef_m[1]); + double latitude = atan2(ecef_m[2], horizontal * (1.0 - eccentricity_squared)); + double height = 0.0; + for(unsigned int iteration = 0 ; iteration < 12u ; iteration++) + { + double sin_latitude = sin(latitude); + double prime_vertical = WGS84_SEMI_MAJOR_M / + sqrt(1.0 - eccentricity_squared * sin_latitude * sin_latitude); + double cos_latitude = cos(latitude); + if(fabs(cos_latitude) < 1.0e-12) + { + height = fabs(ecef_m[2]) - prime_vertical * (1.0 - eccentricity_squared); + } + else + { + height = horizontal / cos_latitude - prime_vertical; + } + double next = atan2(ecef_m[2], + horizontal * (1.0 - eccentricity_squared * + prime_vertical / (prime_vertical + height))); + if(fabs(next - latitude) < 1.0e-13) + { + latitude = next; + break; + } + latitude = next; + } + *latitude_deg_out = degrees(latitude); + *longitude_deg_out = degrees(longitude); + *height_m_out = height; + return llh_valid(*latitude_deg_out, *longitude_deg_out, *height_m_out); +} diff --git a/soc/software/gps_system/src/fusion_motion.c b/soc/software/gps_system/src/fusion_motion.c new file mode 100644 index 0000000..f8bc03c --- /dev/null +++ b/soc/software/gps_system/src/fusion_motion.c @@ -0,0 +1,217 @@ +#include "fusion_motion.h" + +#include +#include +#include + +#define FUSION_MOTION_PI 3.14159265358979323846264338327950288 + +static double norm3(const double vector[3]) +{ + return sqrt(vector[0] * vector[0] + vector[1] * vector[1] + + vector[2] * vector[2]); +} + +static bool finite3(const double vector[3]) +{ + return (vector != NULL) && isfinite(vector[0]) && + isfinite(vector[1]) && isfinite(vector[2]); +} + +static void transition_to(fusion_motion_detector_t* detector, + fusion_motion_state_t state) +{ + if(detector->state != state) + { + detector->state = state; + detector->transition_count++; + } + if(state != FUSION_MOTION_STATIONARY_CANDIDATE) + { + detector->quiet_time_s = 0.0; + } + if(state != FUSION_MOTION_STATIONARY) + { + detector->weak_motion_count = 0u; + } +} + +void fusion_motion_default_config(fusion_motion_config_t* config) +{ + if(config == NULL) + { + return; + } + config->stationary_accel_limit_mps2 = 0.15; + config->stationary_gyro_limit_rps = 1.0 * FUSION_MOTION_PI / 180.0; + config->moving_accel_threshold_mps2 = 0.40; + config->moving_gyro_threshold_rps = 3.0 * FUSION_MOTION_PI / 180.0; + config->moving_jerk_threshold_mps3 = 4.0; + config->jerk_filter_time_constant_s = 0.10; + config->stationary_speed_limit_mps = 0.40; + config->moving_speed_threshold_mps = 0.50; + config->stationary_dwell_s = 1.0; + config->weak_motion_samples = 3u; +} + +void fusion_motion_init(fusion_motion_detector_t* detector, + bool bootstrap_stationary) +{ + if(detector == NULL) + { + return; + } + memset(detector, 0, sizeof(*detector)); + detector->state = bootstrap_stationary + ? FUSION_MOTION_STATIONARY + : FUSION_MOTION_BOOTSTRAP; +} + +fusion_motion_state_t fusion_motion_update( + fusion_motion_detector_t* detector, + const fusion_motion_config_t* config, + const double linear_accel_nav_mps2[3], + const double unbiased_gyro_rps[3], + double speed_mps, + fusion_gnss_motion_evidence_t gnss_evidence, + double dt_s) +{ + if((detector == NULL) || (config == NULL) || + !finite3(linear_accel_nav_mps2) || !finite3(unbiased_gyro_rps) || + !isfinite(speed_mps) || (speed_mps < 0.0) || + !isfinite(dt_s) || (dt_s <= 0.0)) + { + return (detector != NULL) ? detector->state : FUSION_MOTION_BOOTSTRAP; + } + + double accel_norm = norm3(linear_accel_nav_mps2); + double gyro_norm = norm3(unbiased_gyro_rps); + double jerk_norm = 0.0; + if(!detector->have_previous_accel) + { + memcpy(detector->filtered_linear_accel_mps2, linear_accel_nav_mps2, + sizeof(detector->filtered_linear_accel_mps2)); + memcpy(detector->previous_filtered_linear_accel_mps2, + detector->filtered_linear_accel_mps2, + sizeof(detector->previous_filtered_linear_accel_mps2)); + detector->have_previous_accel = true; + } + else + { + double time_constant_s = fmax(0.0, config->jerk_filter_time_constant_s); + double alpha = (time_constant_s > 0.0) + ? dt_s / (time_constant_s + dt_s) + : 1.0; + double delta[3]; + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + detector->previous_filtered_linear_accel_mps2[axis] = + detector->filtered_linear_accel_mps2[axis]; + detector->filtered_linear_accel_mps2[axis] += alpha * + (linear_accel_nav_mps2[axis] - + detector->filtered_linear_accel_mps2[axis]); + delta[axis] = detector->filtered_linear_accel_mps2[axis] - + detector->previous_filtered_linear_accel_mps2[axis]; + } + jerk_norm = norm3(delta) / dt_s; + } + detector->last_linear_accel_norm_mps2 = accel_norm; + detector->last_gyro_norm_rps = gyro_norm; + detector->last_jerk_norm_mps3 = jerk_norm; + detector->last_speed_mps = speed_mps; + + bool gnss_moving = gnss_evidence == FUSION_GNSS_MOTION_MOVING; + bool strong_motion = gnss_moving || + (accel_norm >= config->moving_accel_threshold_mps2) || + (gyro_norm >= config->moving_gyro_threshold_rps) || + (detector->have_previous_accel && + (jerk_norm >= config->moving_jerk_threshold_mps3)) || + (speed_mps >= config->moving_speed_threshold_mps); + bool quiet = (accel_norm <= config->stationary_accel_limit_mps2) && + (gyro_norm <= config->stationary_gyro_limit_rps); + bool stationary_candidate = quiet && !gnss_moving && + (speed_mps <= config->stationary_speed_limit_mps); + bool weak_motion = !quiet; + + switch(detector->state) + { + case FUSION_MOTION_STATIONARY: + if(strong_motion) + { + transition_to(detector, FUSION_MOTION_MOVING); + } + else if(weak_motion) + { + detector->weak_motion_count++; + if(detector->weak_motion_count >= config->weak_motion_samples) + { + transition_to(detector, FUSION_MOTION_MOVING); + } + } + else + { + detector->weak_motion_count = 0u; + } + break; + + case FUSION_MOTION_BOOTSTRAP: + case FUSION_MOTION_MOVING: + if(strong_motion) + { + transition_to(detector, FUSION_MOTION_MOVING); + } + else if(stationary_candidate) + { + transition_to(detector, FUSION_MOTION_STATIONARY_CANDIDATE); + detector->quiet_time_s = dt_s; + } + else + { + transition_to(detector, FUSION_MOTION_MOVING); + } + break; + + case FUSION_MOTION_STATIONARY_CANDIDATE: + if(strong_motion || !stationary_candidate) + { + transition_to(detector, FUSION_MOTION_MOVING); + } + else + { + detector->quiet_time_s += dt_s; + if(detector->quiet_time_s >= config->stationary_dwell_s) + { + transition_to(detector, FUSION_MOTION_STATIONARY); + } + } + break; + + default: + transition_to(detector, FUSION_MOTION_BOOTSTRAP); + break; + } + return detector->state; +} + +bool fusion_motion_allows_zero_update(const fusion_motion_detector_t* detector) +{ + return (detector != NULL) && + (detector->state == FUSION_MOTION_STATIONARY); +} + +const char* fusion_motion_state_name(fusion_motion_state_t state) +{ + switch(state) + { + case FUSION_MOTION_BOOTSTRAP: + return "bootstrap"; + case FUSION_MOTION_STATIONARY: + return "stationary"; + case FUSION_MOTION_MOVING: + return "moving"; + case FUSION_MOTION_STATIONARY_CANDIDATE: + return "stationary_candidate"; + default: + return "unknown"; + } +} diff --git a/soc/software/gps_system/src/gps_fusion_publisher.c b/soc/software/gps_system/src/gps_fusion_publisher.c new file mode 100644 index 0000000..d87131b --- /dev/null +++ b/soc/software/gps_system/src/gps_fusion_publisher.c @@ -0,0 +1,30 @@ +#include "gps_fusion_publisher.h" + +#include "navigation_fusion.h" + +#include +#include + +bool gps_fusion_publish_solution(const gps_nav_solution_t* solution, + uint64_t measurement_timestamp_ticks) +{ + if((solution == NULL) || (measurement_timestamp_ticks == 0u)) + { + return false; + } + fusion_gps_measurement_t measurement; + memset(&measurement, 0, sizeof(measurement)); + measurement.timestamp_ticks = measurement_timestamp_ticks; + measurement.latitude_deg = solution->latitude_deg; + measurement.longitude_deg = solution->longitude_deg; + measurement.height_m = solution->height_m; + memcpy(measurement.covariance_enu_m2, solution->covariance_enu_m2, + sizeof(measurement.covariance_enu_m2)); + measurement.hacc_m = solution->hacc_m; + measurement.vacc_m = solution->vacc_m; + measurement.residual_rms_m = solution->residual_rms_m; + measurement.chi_square_ratio = solution->chi_square_ratio; + measurement.used_satellites = solution->used_satellite_count; + measurement.solution_valid = solution->solution_valid; + return navigation_fusion_publish_gps(&measurement); +} diff --git a/soc/software/gps_system/src/gps_interrupt_handlers.c b/soc/software/gps_system/src/gps_interrupt_handlers.c index 09c9e99..432b899 100644 --- a/soc/software/gps_system/src/gps_interrupt_handlers.c +++ b/soc/software/gps_system/src/gps_interrupt_handlers.c @@ -3,6 +3,7 @@ #include "Navigation.h" #include "gps_pps.h" #include "gps_time_anchor.h" +#include "tdk_icm42688.h" #include #include #include @@ -25,6 +26,7 @@ typedef struct { double corrected_abs_sample; uint32_t nav_solve_timer; uint32_t nav_solve_event_count; + uint64_t local_timestamp_ticks; uint64_t pps_counter; uint32_t pps_capture_sequence; bool pps_capture_valid; @@ -393,6 +395,12 @@ void gps_interrupt_init(void) void gps_dispatch_irq(unsigned int irq_id) { + if(irq_id == BSP_PMOD_IRQ) + { + tdk_icm42688_irq_handler(); + return; + } + unsigned char acq_engine_id; if(gps_irq_to_acq_engine(irq_id, &acq_engine_id)) { @@ -633,6 +641,7 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip, return; } + uint64_t local_timestamp_ticks = bsp_read64(BSP_CLINT_MTIME); uint32_t status_reg = trck_ip->STATUS_REG; if((status_reg & (1u << GPS_TRCK_INTR_PEND_POS)) == 0u) { @@ -653,6 +662,7 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip, event.corrected_abs_sample = gps_correct_abs_sample(event.abs_sample_raw, event.rem_code_phase); event.nav_solve_timer = trck_ip->NAV_SOLVE_TIMER; event.nav_solve_event_count = (status_reg & (1u << GPS_TRCK_NAV_SOLVE_FLAG_POS)) ? 1u : 0u; + event.local_timestamp_ticks = local_timestamp_ticks; if(event.nav_solve_event_count != 0u) { event.pps_capture_valid = gps_pps_read_capture(trck_ip_no, @@ -1032,6 +1042,7 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event) gps_nav_solve_period_pending = true; node->gps_last_nav_solve_abs_sample = event->abs_sample_raw; node->gps_last_nav_solve_corrected_abs_sample = event->corrected_abs_sample; + node->gps_last_nav_solve_local_ticks = event->local_timestamp_ticks; if(event->pps_capture_valid) { node->gps_last_nav_solve_pps_counter = event->pps_counter; diff --git a/soc/software/gps_system/src/gps_ip.c b/soc/software/gps_system/src/gps_ip.c index 044ea4a..15ef762 100644 --- a/soc/software/gps_system/src/gps_ip.c +++ b/soc/software/gps_system/src/gps_ip.c @@ -9,6 +9,7 @@ #include "linked_list.h" #include "gps_interrupt_handlers.h" #include "gps_pps.h" +#include "gps_fusion_publisher.h" #include "defines.h" #include @@ -766,6 +767,7 @@ static void gps_capture_nav_metadata_locked(const unsigned char snapshot_ips[GPS uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT], uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT], uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT], + uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT], unsigned int satellite_count) { for(unsigned int i = 0 ; i < satellite_count ; i++) @@ -774,9 +776,34 @@ static void gps_capture_nav_metadata_locked(const unsigned char snapshot_ips[GPS snapshot_sequences[i] = (node != NULL) ? node->gps_nav_solve_sequence : 0u; snapshot_pps_counters[i] = (node != NULL) ? node->gps_last_nav_solve_pps_counter : 0u; snapshot_pps_sequences[i] = (node != NULL) ? node->gps_last_pps_capture_sequence : 0u; + snapshot_local_timestamps[i] = + (node != NULL) ? node->gps_last_nav_solve_local_ticks : 0u; } } +static uint64_t gps_nav_median_local_timestamp( + const uint64_t timestamps[GPS_TRCK_IP_COUNT], unsigned int count) +{ + uint64_t sorted[GPS_TRCK_IP_COUNT]; + unsigned int valid_count = 0u; + for(unsigned int i = 0 ; (i < count) && (i < GPS_TRCK_IP_COUNT) ; i++) + { + if(timestamps[i] == 0u) + { + continue; + } + unsigned int insert = valid_count; + while((insert != 0u) && (sorted[insert - 1u] > timestamps[i])) + { + sorted[insert] = sorted[insert - 1u]; + insert--; + } + sorted[insert] = timestamps[i]; + valid_count++; + } + return (valid_count != 0u) ? sorted[valid_count / 2u] : 0u; +} + static void gps_consume_nav_sequences_locked(const unsigned char snapshot_ips[GPS_TRCK_IP_COUNT], const uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT], unsigned int satellite_count) @@ -1069,6 +1096,7 @@ void calculate_position() uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT]; uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT]; uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT]; + uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT]; unsigned int satellite_count = 0; bool run_nav = false; bool initial_run = false; @@ -1115,6 +1143,7 @@ void calculate_position() { gps_capture_nav_metadata_locked(snapshot_ips, snapshot_sequences, snapshot_pps_counters, snapshot_pps_sequences, + snapshot_local_timestamps, satellite_count); } } @@ -1157,6 +1186,14 @@ void calculate_position() uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles; if(nav_ok) { + uint64_t measurement_timestamp = gps_nav_median_local_timestamp( + snapshot_local_timestamps, satellite_count); + if(!gps_fusion_publish_solution(&nav_solution, measurement_timestamp)) + { + GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, + "fusion GPS publish skipped timestamp=%lu\r\n", + (unsigned long)measurement_timestamp); + } uint64_t consume_irq_state = bsp_irq_save(); gps_consume_nav_sequences_locked(snapshot_ips, snapshot_sequences, satellite_count); bsp_irq_restore(consume_irq_state); diff --git a/soc/software/gps_system/src/linked_list.c b/soc/software/gps_system/src/linked_list.c index ebc44f6..afb15ee 100644 --- a/soc/software/gps_system/src/linked_list.c +++ b/soc/software/gps_system/src/linked_list.c @@ -70,6 +70,7 @@ bool insert_element(volatile gps_ephem_ll_t** node, new_node->gps_nav_solve_consumed_sequence = 0u; new_node->gps_last_nav_solve_abs_sample = 0u; new_node->gps_last_nav_solve_corrected_abs_sample = 0.0; + new_node->gps_last_nav_solve_local_ticks = 0u; new_node->gps_last_nav_solve_pps_counter = 0u; new_node->gps_last_pps_capture_sequence = 0u; new_node->gps_last_measurement_tick = 0u; diff --git a/soc/software/gps_system/src/navigation_fusion.c b/soc/software/gps_system/src/navigation_fusion.c new file mode 100644 index 0000000..cd6ee99 --- /dev/null +++ b/soc/software/gps_system/src/navigation_fusion.c @@ -0,0 +1,108 @@ +#include "navigation_fusion.h" + +#include "fusion_bus.h" +#include "fusion_gps_adapter.h" +#include "tdk_icm42688.h" + +#include +#include +#include + +static fusion_bus_t measurement_bus; +static fusion_gps_adapter_t gps_adapter; +static fusion_engine_t fusion_engine; +static bool fusion_initialized; + +bool navigation_fusion_init(uint64_t local_timebase_hz) +{ + fusion_engine_config_t engine_config; + fusion_gps_adapter_config_t adapter_config; + if(local_timebase_hz == 0u) + { + return false; + } + fusion_bus_init(&measurement_bus); + fusion_engine_default_config(&engine_config, local_timebase_hz); + fusion_gps_adapter_default_config(&adapter_config); + if(!fusion_engine_init(&fusion_engine, &engine_config) || + !fusion_gps_adapter_init(&gps_adapter, &adapter_config)) + { + return false; + } + fusion_initialized = true; + return true; +} + +bool navigation_fusion_publish_gps(const fusion_gps_measurement_t* measurement) +{ + return fusion_initialized && + fusion_bus_publish_gps(&measurement_bus, measurement); +} + +unsigned int navigation_fusion_service(void) +{ + if(!fusion_initialized) + { + return 0u; + } + unsigned int processed = 0u; + fusion_imu_measurement_t imu; + while(tdk_icm42688_take_measurement(&imu)) + { + (void)fusion_engine_push_imu(&fusion_engine, &imu); + processed++; + } + + /* GPS is intentionally consumed after all currently queued IMU samples. + * A delayed WLS result is applied at its own timestamp by fixed-lag replay. */ + fusion_gps_measurement_t gps; + while(fusion_bus_take_gps(&measurement_bus, &gps)) + { + fusion_position_measurement_t position; + if(fusion_gps_adapter_convert(&gps_adapter, &gps, &position)) + { + (void)fusion_engine_push_position(&fusion_engine, &position, NULL, NULL); + } + processed++; + } + return processed; +} + +bool navigation_fusion_get_output(navigation_fusion_output_t* output) +{ + if((output == NULL) || !fusion_initialized || + (fusion_engine.state != FUSION_ENGINE_RUNNING)) + { + return false; + } + memset(output, 0, sizeof(*output)); + const fusion_runtime_t* runtime = &fusion_engine.current; + output->timestamp_ticks = runtime->filter_timestamp_ticks; + memcpy(output->position_enu_m, runtime->filter.position_m, + sizeof(output->position_enu_m)); + memcpy(output->velocity_enu_mps, runtime->filter.velocity_mps, + sizeof(output->velocity_enu_mps)); + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + output->position_std_m[axis] = + sqrt(fmax(0.0, runtime->filter.covariance[axis][axis])); + } + output->speed_mps = eskf_speed_mps(&runtime->filter); + output->last_position_nis = runtime->filter.last_position_nis; + output->motion_state = runtime->motion.state; + output->gps_accepted = fusion_engine.diagnostics.position_accepted; + output->gps_nis_rejected = fusion_engine.diagnostics.position_nis_rejected; + if(!fusion_gps_adapter_local_to_geodetic( + &gps_adapter, runtime->filter.position_m, + &output->latitude_deg, &output->longitude_deg, &output->height_m)) + { + return false; + } + output->valid = true; + return true; +} + +const fusion_engine_t* navigation_fusion_engine(void) +{ + return fusion_initialized ? &fusion_engine : NULL; +} diff --git a/soc/software/gps_system/src/tdk_icm42688.c b/soc/software/gps_system/src/tdk_icm42688.c new file mode 100644 index 0000000..9733e03 --- /dev/null +++ b/soc/software/gps_system/src/tdk_icm42688.c @@ -0,0 +1,393 @@ +#include "tdk_icm42688.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#define ICM_DEVICE_CONFIG 0x11u +#define ICM_INT_CONFIG 0x14u +#define ICM_TEMP_DATA1 0x1du +#define ICM_INT_STATUS 0x2du +#define ICM_PWR_MGMT0 0x4eu +#define ICM_GYRO_CONFIG0 0x4fu +#define ICM_ACCEL_CONFIG0 0x50u +#define ICM_GYRO_CONFIG1 0x51u +#define ICM_GYRO_ACCEL_CONFIG0 0x52u +#define ICM_ACCEL_CONFIG1 0x53u +#define ICM_INT_CONFIG0 0x63u +#define ICM_INT_CONFIG1 0x64u +#define ICM_INT_SOURCE0 0x65u +#define ICM_WHO_AM_I 0x75u + +#define ICM_SPI_READ 0x80u +#define ICM_SPI_DUMMY 0xffu +#define ICM_DATA_READY 0x08u +#define ICM42688_WHO_AM_I 0x47u +#define ICM_PWR_LOW_NOISE 0x0fu +#define ICM_INT1_LATCHED_HIGH_PUSH_PULL 0x07u +#define ICM_UI_DRDY_INT1_EN 0x08u + +#define TDK_GRAVITY_MPS2 9.80665 +#define TDK_DEGREES_TO_RADIANS 0.017453292519943295769236907684886 + +#if (TDK_IMU_RAW_QUEUE_CAPACITY < 2u) +#error "TDK_IMU_RAW_QUEUE_CAPACITY must be at least 2" +#endif + +static tdk_icm42688_config_t active_config; +static tdk_icm42688_raw_sample_t sample_queue[TDK_IMU_RAW_QUEUE_CAPACITY]; +static volatile unsigned int sample_head; +static volatile unsigned int sample_tail; +static volatile tdk_icm42688_diagnostics_t diagnostics; + +static uint64_t time_now(void) +{ + return bsp_read64(BSP_CLINT_MTIME); +} + +static void delay_us(uint32_t microseconds) +{ + uint64_t ticks = ((uint64_t)BSP_CLINT_TIMEBASE_HZ * microseconds + 999999u) / + 1000000u; + uint64_t start = time_now(); + while((time_now() - start) < ticks) + { + } +} + +static void delay_ms(uint32_t milliseconds) +{ + uint64_t ticks = ((uint64_t)BSP_CLINT_TIMEBASE_HZ * milliseconds + 999u) / + 1000u; + uint64_t start = time_now(); + while((time_now() - start) < ticks) + { + } +} + +static void write_register(uint8_t reg, uint8_t value) +{ + bsp_pmod_spi_write_reg8(reg & (uint8_t)~ICM_SPI_READ, value); +} + +static uint8_t read_register(uint8_t reg) +{ + return bsp_pmod_spi_read_reg8(reg | ICM_SPI_READ, ICM_SPI_DUMMY); +} + +static int16_t big_endian_i16(const uint8_t* bytes) +{ + return (int16_t)(((uint16_t)bytes[0] << 8) | bytes[1]); +} + +static bool odr_register_value(uint32_t odr_hz, uint8_t* register_value) +{ + if(register_value == NULL) + { + return false; + } + switch(odr_hz) + { + case 1000u: *register_value = 0x06u; return true; + case 500u: *register_value = 0x0fu; return true; + case 200u: *register_value = 0x07u; return true; + case 100u: *register_value = 0x08u; return true; + case 50u: *register_value = 0x09u; return true; + case 25u: *register_value = 0x0au; return true; + default: return false; + } +} + +static bool accel_full_scale_bits(tdk_accel_full_scale_t full_scale, + uint8_t* bits) +{ + if(bits == NULL) + { + return false; + } + switch(full_scale) + { + case TDK_ACCEL_FS_16G: *bits = 0u; return true; + case TDK_ACCEL_FS_8G: *bits = 1u; return true; + case TDK_ACCEL_FS_4G: *bits = 2u; return true; + case TDK_ACCEL_FS_2G: *bits = 3u; return true; + default: return false; + } +} + +static bool gyro_full_scale_bits(tdk_gyro_full_scale_mdps_t full_scale, + uint8_t* bits) +{ + if(bits == NULL) + { + return false; + } + switch(full_scale) + { + case TDK_GYRO_FS_2000: *bits = 0u; return true; + case TDK_GYRO_FS_1000: *bits = 1u; return true; + case TDK_GYRO_FS_500: *bits = 2u; return true; + case TDK_GYRO_FS_250: *bits = 3u; return true; + case TDK_GYRO_FS_125: *bits = 4u; return true; + case TDK_GYRO_FS_62D5: *bits = 5u; return true; + case TDK_GYRO_FS_31D25: *bits = 6u; return true; + case TDK_GYRO_FS_15D625: *bits = 7u; return true; + default: return false; + } +} + +static unsigned int next_queue_index(unsigned int index) +{ + index++; + return (index == TDK_IMU_RAW_QUEUE_CAPACITY) ? 0u : index; +} + +static bool raw_sample_valid(const tdk_icm42688_raw_sample_t* sample) +{ + if((sample == NULL) || (sample->temperature_raw == INT16_MIN)) + { + return false; + } + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + if((sample->accel_raw[axis] == INT16_MIN) || + (sample->gyro_raw[axis] == INT16_MIN)) + { + return false; + } + } + return true; +} + +static bool queue_push_isr(const tdk_icm42688_raw_sample_t* sample) +{ + unsigned int head = __atomic_load_n(&sample_head, __ATOMIC_RELAXED); + unsigned int next = next_queue_index(head); + if(next == __atomic_load_n(&sample_tail, __ATOMIC_ACQUIRE)) + { + __atomic_fetch_add(&diagnostics.queue_overflow_count, 1u, __ATOMIC_RELAXED); + return false; + } + sample_queue[head] = *sample; + __atomic_store_n(&sample_head, next, __ATOMIC_RELEASE); + return true; +} + +static bool queue_pop(tdk_icm42688_raw_sample_t* sample) +{ + unsigned int tail = __atomic_load_n(&sample_tail, __ATOMIC_RELAXED); + if(tail == __atomic_load_n(&sample_head, __ATOMIC_ACQUIRE)) + { + return false; + } + *sample = sample_queue[tail]; + __atomic_store_n(&sample_tail, next_queue_index(tail), __ATOMIC_RELEASE); + return true; +} + +void tdk_icm42688_default_config(tdk_icm42688_config_t* config) +{ + if(config == NULL) + { + return; + } + config->odr_hz = TDK_IMU_ODR_HZ; + /* Motion-capable defaults; stationary characterization may select narrower ranges. */ + config->accel_full_scale_g = TDK_ACCEL_FS_4G; + config->gyro_full_scale_mdps = TDK_GYRO_FS_250; + config->ui_filter_config = 0x77u; +} + +bool tdk_icm42688_init(const tdk_icm42688_config_t* config) +{ + uint8_t odr_bits; + uint8_t accel_bits; + uint8_t gyro_bits; + if((config == NULL) || !odr_register_value(config->odr_hz, &odr_bits) || + !accel_full_scale_bits(config->accel_full_scale_g, &accel_bits) || + !gyro_full_scale_bits(config->gyro_full_scale_mdps, &gyro_bits)) + { + return false; + } + memset((void*)&diagnostics, 0, sizeof(diagnostics)); + sample_head = 0u; + sample_tail = 0u; + active_config = *config; + + bsp_pmod_spi_init(); + bsp_pmod_spi_set_mode(BSP_PMOD_SPI_MODE_SINGLE); + bsp_pmod_spi_set_timing(10u, 10u, 10u, 10u); + delay_us(1000u); + diagnostics.who_am_i = read_register(ICM_WHO_AM_I); + if((diagnostics.who_am_i != ICM42688_WHO_AM_I) || + (read_register(ICM_DEVICE_CONFIG) != 0x00u)) + { + return false; + } + + write_register(ICM_INT_SOURCE0, 0x00u); + write_register(ICM_PWR_MGMT0, 0x00u); + delay_ms(1u); + write_register(ICM_GYRO_CONFIG0, (uint8_t)((gyro_bits << 5) | odr_bits)); + write_register(ICM_ACCEL_CONFIG0, (uint8_t)((accel_bits << 5) | odr_bits)); + write_register(ICM_GYRO_CONFIG1, + (uint8_t)((read_register(ICM_GYRO_CONFIG1) & 0x10u) | 0xcau)); + write_register(ICM_ACCEL_CONFIG1, + (uint8_t)((read_register(ICM_ACCEL_CONFIG1) & 0xe1u) | 0x14u)); + write_register(ICM_GYRO_ACCEL_CONFIG0, config->ui_filter_config); + + /* Active-high, push-pull, latched INT1. Status read clears data-ready. */ + write_register(ICM_INT_CONFIG, ICM_INT1_LATCHED_HIGH_PUSH_PULL); + write_register(ICM_INT_CONFIG0, 0x00u); + /* Datasheet requires clearing the reset default bit 4 for pin operation. */ + write_register(ICM_INT_CONFIG1, 0x00u); + write_register(ICM_PWR_MGMT0, ICM_PWR_LOW_NOISE); + delay_ms(50u); + + if((read_register(ICM_PWR_MGMT0) != ICM_PWR_LOW_NOISE) || + (read_register(ICM_GYRO_CONFIG0) != (uint8_t)((gyro_bits << 5) | odr_bits)) || + (read_register(ICM_ACCEL_CONFIG0) != (uint8_t)((accel_bits << 5) | odr_bits)) || + (read_register(ICM_GYRO_ACCEL_CONFIG0) != config->ui_filter_config) || + (read_register(ICM_INT_CONFIG) != ICM_INT1_LATCHED_HIGH_PUSH_PULL) || + (read_register(ICM_INT_CONFIG1) != 0x00u)) + { + write_register(ICM_PWR_MGMT0, 0x00u); + return false; + } + (void)read_register(ICM_INT_STATUS); + diagnostics.initialized = true; + return true; +} + +bool tdk_icm42688_start(void) +{ + if(!diagnostics.initialized || diagnostics.running) + { + return diagnostics.running; + } + (void)read_register(ICM_INT_STATUS); + bsp_plic_set_priority(BSP_PMOD_IRQ, 1u); + write_register(ICM_INT_SOURCE0, ICM_UI_DRDY_INT1_EN); + if(read_register(ICM_INT_SOURCE0) != ICM_UI_DRDY_INT1_EN) + { + bsp_plic_disable(BSP_PMOD_IRQ); + return false; + } + (void)read_register(ICM_INT_STATUS); + diagnostics.running = true; + bsp_plic_enable(BSP_PMOD_IRQ); + return true; +} + +void tdk_icm42688_stop(void) +{ + bsp_plic_disable(BSP_PMOD_IRQ); + write_register(ICM_INT_SOURCE0, 0x00u); + (void)read_register(ICM_INT_STATUS); + diagnostics.running = false; +} + +void tdk_icm42688_irq_handler(void) +{ + uint64_t timestamp = time_now(); + __atomic_fetch_add(&diagnostics.irq_count, 1u, __ATOMIC_RELAXED); + uint8_t status = read_register(ICM_INT_STATUS); + if(!diagnostics.running || ((status & ICM_DATA_READY) == 0u)) + { + __atomic_fetch_add(&diagnostics.spurious_irq_count, 1u, __ATOMIC_RELAXED); + return; + } + uint8_t bytes[14]; + bsp_pmod_spi_read_regs(ICM_TEMP_DATA1 | ICM_SPI_READ, ICM_SPI_DUMMY, + bytes, sizeof(bytes)); + tdk_icm42688_raw_sample_t sample; + sample.timestamp_ticks = timestamp; + sample.temperature_raw = big_endian_i16(&bytes[0]); + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + sample.accel_raw[axis] = big_endian_i16(&bytes[2u + 2u * axis]); + sample.gyro_raw[axis] = big_endian_i16(&bytes[8u + 2u * axis]); + } + sample.sequence = (uint32_t)(__atomic_load_n(&diagnostics.sample_count, + __ATOMIC_RELAXED) + 1u); + if(!raw_sample_valid(&sample)) + { + __atomic_fetch_add(&diagnostics.invalid_sample_count, 1u, __ATOMIC_RELAXED); + return; + } + uint64_t previous_timestamp = + __atomic_exchange_n(&diagnostics.last_timestamp_ticks, timestamp, + __ATOMIC_RELAXED); + if(previous_timestamp != 0u) + { + uint64_t expected_ticks = BSP_CLINT_TIMEBASE_HZ / active_config.odr_hz; + uint64_t elapsed = timestamp - previous_timestamp; + if((expected_ticks != 0u) && (elapsed > expected_ticks + expected_ticks / 2u)) + { + uint64_t periods = (elapsed + expected_ticks / 2u) / expected_ticks; + if(periods > 1u) + { + __atomic_fetch_add(&diagnostics.estimated_missed_sample_count, + periods - 1u, __ATOMIC_RELAXED); + } + } + } + (void)queue_push_isr(&sample); + __atomic_fetch_add(&diagnostics.sample_count, 1u, __ATOMIC_RELAXED); +} + +bool tdk_icm42688_take_measurement(fusion_imu_measurement_t* measurement_out) +{ + if(measurement_out == NULL) + { + return false; + } + tdk_icm42688_raw_sample_t raw; + if(!queue_pop(&raw)) + { + return false; + } + double accel_scale = (double)active_config.accel_full_scale_g * + TDK_GRAVITY_MPS2 / 32768.0; + double gyro_scale = ((double)active_config.gyro_full_scale_mdps / 1000.0) * + TDK_DEGREES_TO_RADIANS / 32768.0; + measurement_out->timestamp_ticks = raw.timestamp_ticks; + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + measurement_out->accel_mps2[axis] = (double)raw.accel_raw[axis] * accel_scale; + measurement_out->gyro_rps[axis] = (double)raw.gyro_raw[axis] * gyro_scale; + } + measurement_out->temperature_c = 25.0 + (double)raw.temperature_raw / 132.48; + measurement_out->sequence = raw.sequence; + return true; +} + +void tdk_icm42688_get_diagnostics(tdk_icm42688_diagnostics_t* diagnostics_out) +{ + if(diagnostics_out == NULL) + { + return; + } + diagnostics_out->irq_count = + __atomic_load_n(&diagnostics.irq_count, __ATOMIC_RELAXED); + diagnostics_out->sample_count = + __atomic_load_n(&diagnostics.sample_count, __ATOMIC_RELAXED); + diagnostics_out->invalid_sample_count = + __atomic_load_n(&diagnostics.invalid_sample_count, __ATOMIC_RELAXED); + diagnostics_out->spurious_irq_count = + __atomic_load_n(&diagnostics.spurious_irq_count, __ATOMIC_RELAXED); + diagnostics_out->queue_overflow_count = + __atomic_load_n(&diagnostics.queue_overflow_count, __ATOMIC_RELAXED); + diagnostics_out->estimated_missed_sample_count = + __atomic_load_n(&diagnostics.estimated_missed_sample_count, __ATOMIC_RELAXED); + diagnostics_out->last_timestamp_ticks = + __atomic_load_n(&diagnostics.last_timestamp_ticks, __ATOMIC_RELAXED); + diagnostics_out->who_am_i = diagnostics.who_am_i; + diagnostics_out->initialized = diagnostics.initialized; + diagnostics_out->running = diagnostics.running; +}