Integrate asynchronous GPS IMU fusion
This commit is contained in:
@@ -350,8 +350,12 @@ module tracking_wrapper #(
|
||||
end : preamble_search_comb
|
||||
|
||||
logic [301 : 0] shift_reg_resultant;
|
||||
logic [16 : 0] how_z_count;
|
||||
|
||||
assign shift_reg_resultant[301-:2] = shift_reg_p[309-:2]; // always assign last 2 bit from previous subframe
|
||||
/* Normalized HOW word is nav word 2 at [269:240]; bits [29:13] are
|
||||
* the 17-bit Z-count used to align NAV solve timer phase globally. */
|
||||
assign how_z_count = shift_reg_resultant[269-:17];
|
||||
|
||||
generate
|
||||
genvar j;
|
||||
@@ -457,6 +461,7 @@ module tracking_wrapper #(
|
||||
.counter_max_val(nav_solve_timer_reg_p), // timer generates interrupt when internal timer reaches this value.
|
||||
.bit_valid(bit_valid_syned), // timer counts bit valid signals.
|
||||
.subframe_ready(load_sigs), // signal that asserts 1 clock cycle after a subframe bit reached ( in essence, 300 bit + 8 preamble bit !!!)
|
||||
.subframe_count(how_z_count), // HOW Z-count of this globally identified subframe boundary.
|
||||
.trck_busy(busy_syned), // stop counting when tracking is unlocked or do not start to count anyways.
|
||||
// output from module.
|
||||
.nav_solve_timer_reached(nav_solve_timer_reached), // timer internal count is equal to predefined count value. (status flag) asserted 1 clock cycle. INTERRUPT SIGNAL TO CPU FROM TIMER TODO: will be integrated to main interrupt generation mechanishm.
|
||||
|
||||
+51
-2
@@ -6,6 +6,7 @@ module nav_solve_timer(
|
||||
input logic [31 : 0] counter_max_val,
|
||||
input logic bit_valid,
|
||||
input logic subframe_ready,
|
||||
input logic [16 : 0] subframe_count,
|
||||
input logic trck_busy,
|
||||
output logic nav_solve_timer_reached,
|
||||
output logic load_abs_sample
|
||||
@@ -28,12 +29,20 @@ module nav_solve_timer(
|
||||
assign nav_solve_timer_reached = intr_delay_ff;
|
||||
|
||||
logic [31 : 0] internal_counter_p, internal_counter_n;
|
||||
logic [25 : 0] phase_dividend_p, phase_dividend_n;
|
||||
logic [31 : 0] phase_remainder_p, phase_remainder_n;
|
||||
logic [4 : 0] phase_bit_p, phase_bit_n;
|
||||
logic [32 : 0] phase_candidate;
|
||||
logic [32 : 0] phase_reduced;
|
||||
|
||||
nav_solve_timer_pkg::nav_solve_timer_e nav_solve_state_p, nav_solve_state_n;
|
||||
|
||||
always_ff @(posedge clk or negedge resetn) begin : counter_seq_logic
|
||||
if(!resetn) begin
|
||||
internal_counter_p <= '0;
|
||||
phase_dividend_p <= '0;
|
||||
phase_remainder_p <= '0;
|
||||
phase_bit_p <= '0;
|
||||
bit_valid_cc_p <= '0;
|
||||
abs_sample_load_cc_p <= '0;
|
||||
nav_solve_state_p <= nav_solve_timer_pkg::IDLE;
|
||||
@@ -41,6 +50,9 @@ module nav_solve_timer(
|
||||
end
|
||||
else begin
|
||||
internal_counter_p <= internal_counter_n;
|
||||
phase_dividend_p <= phase_dividend_n;
|
||||
phase_remainder_p <= phase_remainder_n;
|
||||
phase_bit_p <= phase_bit_n;
|
||||
bit_valid_cc_p <= bit_valid_cc_n;
|
||||
abs_sample_load_cc_p <= abs_sample_load_cc_n;
|
||||
nav_solve_state_p <= nav_solve_state_n;
|
||||
@@ -50,10 +62,47 @@ module nav_solve_timer(
|
||||
|
||||
always_comb begin : counter_comb_logic
|
||||
internal_counter_n = internal_counter_p;
|
||||
phase_dividend_n = phase_dividend_p;
|
||||
phase_remainder_n = phase_remainder_p;
|
||||
phase_bit_n = phase_bit_p;
|
||||
phase_candidate = '0;
|
||||
phase_reduced = '0;
|
||||
load_abs_sample_internal = '0;
|
||||
nav_solve_state_n = nav_solve_state_p;
|
||||
if((nav_solve_state_p == nav_solve_timer_pkg::IDLE) && (subframe_ready) && (trck_busy) && (enable)) begin // subframe ready is 1 cc pulse signal.
|
||||
nav_solve_state_n = nav_solve_timer_pkg::COUNT;
|
||||
if((nav_solve_state_p == nav_solve_timer_pkg::IDLE) && subframe_ready && trck_busy && enable && (counter_max_val != 0)) begin
|
||||
/* HOW Z-count identifies this boundary globally in six-second
|
||||
* subframes. Seed the per-channel counter with the corresponding
|
||||
* navigation-bit phase so channels which lock on different
|
||||
* subframes still produce the same NAV solve epochs for any period.
|
||||
* Multiplication by 300 is constant shift/add; modulo is evaluated in
|
||||
* ALIGN over 26 PCLK cycles, far below one 20 ms navigation bit. */
|
||||
phase_dividend_n = ({9'b0, subframe_count} << 8) +
|
||||
({9'b0, subframe_count} << 5) +
|
||||
({9'b0, subframe_count} << 3) +
|
||||
({9'b0, subframe_count} << 2);
|
||||
phase_remainder_n = '0;
|
||||
phase_bit_n = 5'd25;
|
||||
nav_solve_state_n = nav_solve_timer_pkg::ALIGN;
|
||||
end
|
||||
if(nav_solve_state_p == nav_solve_timer_pkg::ALIGN) begin
|
||||
if(!enable || !trck_busy || (counter_max_val == 0)) begin
|
||||
nav_solve_state_n = nav_solve_timer_pkg::IDLE;
|
||||
end
|
||||
else begin
|
||||
phase_candidate = ({1'b0, phase_remainder_p} << 1) |
|
||||
phase_dividend_p[phase_bit_p];
|
||||
phase_reduced = (phase_candidate >= {1'b0, counter_max_val})
|
||||
? (phase_candidate - {1'b0, counter_max_val})
|
||||
: phase_candidate;
|
||||
phase_remainder_n = phase_reduced[31:0];
|
||||
if(phase_bit_p == 0) begin
|
||||
internal_counter_n = phase_reduced[31:0];
|
||||
nav_solve_state_n = nav_solve_timer_pkg::COUNT;
|
||||
end
|
||||
else begin
|
||||
phase_bit_n = phase_bit_p - 1'b1;
|
||||
end
|
||||
end
|
||||
end
|
||||
if(nav_solve_state_p == nav_solve_timer_pkg::COUNT) begin
|
||||
if(!enable) nav_solve_state_n = nav_solve_timer_pkg::IDLE;
|
||||
|
||||
@@ -1,9 +1,10 @@
|
||||
|
||||
package nav_solve_timer_pkg;
|
||||
|
||||
typedef enum{
|
||||
IDLE = 0,
|
||||
COUNT = 1
|
||||
}nav_solve_timer_e;
|
||||
typedef enum logic [1 : 0] {
|
||||
IDLE = 2'd0,
|
||||
ALIGN = 2'd1,
|
||||
COUNT = 2'd2
|
||||
} nav_solve_timer_e;
|
||||
|
||||
endpackage
|
||||
|
||||
+21
@@ -0,0 +1,21 @@
|
||||
QUESTA_VLOG ?= vlog
|
||||
QUESTA_VSIM ?= vsim
|
||||
QUESTA_RUN_DIR ?= /tmp/gps_nav_solve_timer_questa
|
||||
|
||||
TIMER_DIR := $(abspath ..)
|
||||
|
||||
.PHONY: test clean
|
||||
|
||||
test:
|
||||
rm -rf $(QUESTA_RUN_DIR)
|
||||
mkdir -p $(QUESTA_RUN_DIR)
|
||||
cd $(QUESTA_RUN_DIR) && vlib work
|
||||
cd $(QUESTA_RUN_DIR) && $(QUESTA_VLOG) \
|
||||
$(TIMER_DIR)/nav_solve_timer_pkg.sv \
|
||||
$(TIMER_DIR)/nav_solve_timer.sv \
|
||||
$(CURDIR)/nav_solve_timer_tb.sv
|
||||
cd $(QUESTA_RUN_DIR) && $(QUESTA_VSIM) -c nav_solve_timer_tb \
|
||||
-do 'run -all; quit -f'
|
||||
|
||||
clean:
|
||||
rm -rf $(QUESTA_RUN_DIR)
|
||||
@@ -0,0 +1,107 @@
|
||||
`timescale 1ns/1ps
|
||||
|
||||
module nav_solve_timer_tb;
|
||||
logic clk = 1'b0;
|
||||
logic resetn = 1'b0;
|
||||
logic enable = 1'b1;
|
||||
logic [31:0] counter_max_val;
|
||||
logic bit_valid = 1'b0;
|
||||
logic subframe_ready_a = 1'b0;
|
||||
logic subframe_ready_b = 1'b0;
|
||||
logic [16:0] subframe_count_a = '0;
|
||||
logic [16:0] subframe_count_b = '0;
|
||||
logic trck_busy = 1'b1;
|
||||
logic reached_a;
|
||||
logic reached_b;
|
||||
logic load_a;
|
||||
logic load_b;
|
||||
|
||||
int unsigned global_bit;
|
||||
bit b_active;
|
||||
|
||||
always #5 clk = ~clk;
|
||||
|
||||
nav_solve_timer timer_a (
|
||||
.clk, .resetn, .enable, .counter_max_val, .bit_valid,
|
||||
.subframe_ready(subframe_ready_a),
|
||||
.subframe_count(subframe_count_a),
|
||||
.trck_busy, .nav_solve_timer_reached(reached_a),
|
||||
.load_abs_sample(load_a)
|
||||
);
|
||||
|
||||
nav_solve_timer timer_b (
|
||||
.clk, .resetn, .enable, .counter_max_val, .bit_valid,
|
||||
.subframe_ready(subframe_ready_b),
|
||||
.subframe_count(subframe_count_b),
|
||||
.trck_busy, .nav_solve_timer_reached(reached_b),
|
||||
.load_abs_sample(load_b)
|
||||
);
|
||||
|
||||
task automatic clock_cycles(input int unsigned count);
|
||||
repeat(count) @(posedge clk);
|
||||
endtask
|
||||
|
||||
task automatic pulse_subframe(input bit start_a, input bit start_b,
|
||||
input logic [16:0] z_count);
|
||||
subframe_count_a = z_count;
|
||||
subframe_count_b = z_count;
|
||||
subframe_ready_a = start_a;
|
||||
subframe_ready_b = start_b;
|
||||
@(posedge clk);
|
||||
subframe_ready_a = 1'b0;
|
||||
subframe_ready_b = 1'b0;
|
||||
/* ALIGN consumes 26 PCLK cycles before the next 20 ms bit event. */
|
||||
clock_cycles(30);
|
||||
endtask
|
||||
|
||||
task automatic pulse_nav_bit;
|
||||
bit saw_a;
|
||||
bit saw_b;
|
||||
bit expected;
|
||||
saw_a = 1'b0;
|
||||
saw_b = 1'b0;
|
||||
bit_valid = 1'b1;
|
||||
@(posedge clk);
|
||||
bit_valid = 1'b0;
|
||||
repeat(8) begin
|
||||
@(posedge clk);
|
||||
saw_a |= reached_a;
|
||||
saw_b |= reached_b;
|
||||
end
|
||||
global_bit++;
|
||||
expected = ((global_bit % counter_max_val) == 0);
|
||||
if(saw_a !== expected)
|
||||
$fatal(1, "timer A phase mismatch: N=%0d bit=%0d got=%0d expected=%0d",
|
||||
counter_max_val, global_bit, saw_a, expected);
|
||||
if(b_active && (saw_b !== expected))
|
||||
$fatal(1, "timer B phase mismatch: N=%0d bit=%0d got=%0d expected=%0d",
|
||||
counter_max_val, global_bit, saw_b, expected);
|
||||
endtask
|
||||
|
||||
task automatic run_late_join_scenario(input int unsigned period);
|
||||
counter_max_val = period;
|
||||
global_bit = 100 * 300;
|
||||
b_active = 1'b0;
|
||||
resetn = 1'b0;
|
||||
clock_cycles(3);
|
||||
resetn = 1'b1;
|
||||
clock_cycles(2);
|
||||
|
||||
pulse_subframe(1'b1, 1'b0, 17'd100);
|
||||
repeat(300) pulse_nav_bit();
|
||||
|
||||
/* Timer B locks one complete six-second subframe later. */
|
||||
b_active = 1'b1;
|
||||
pulse_subframe(1'b0, 1'b1, 17'd101);
|
||||
repeat(period * 3) pulse_nav_bit();
|
||||
endtask
|
||||
|
||||
initial begin
|
||||
run_late_join_scenario(40);
|
||||
run_late_join_scenario(37);
|
||||
run_late_join_scenario(512);
|
||||
run_late_join_scenario(1);
|
||||
$display("nav_solve_timer_tb PASS");
|
||||
$finish;
|
||||
end
|
||||
endmodule
|
||||
+135
-13
@@ -12,6 +12,55 @@ Bu bölüm yeni bir geliştirme oturumunun nereden devam edeceğini tek yerde
|
||||
özetler. Aşağıdaki Python model ve bare-metal C uygulaması aynı problemi çözer,
|
||||
fakat aynı program değildir:
|
||||
|
||||
### Asenkron IMU/GPS düzeltmesi ve sentetik oran matrisi
|
||||
|
||||
Önceki prototipte 52 Hz ST akışından yalnız yaklaşık 250 ms'de bir ham örnek
|
||||
seçiliyordu. Bu inertial downsampling değildir: kısa gyro hareketi iki seçilen
|
||||
örneğin arasında kalınca değişen gravity vektörü görülüyor, dönüş görülmüyor ve
|
||||
yanlış yatay ivme entegre ediliyordu. Linux C hızlı-eğim testinde bu yol 8.169 m
|
||||
tepe yatay hata ve 1.554 m/s kalıcı hız üretti.
|
||||
|
||||
Bu seçim katmanı kaldırıldı. Artık her raw sample kendi timestamp'iyle işlenir;
|
||||
ardışık accel/gyro örnekleri midpoint mechanization ile entegre edilir. GPS,
|
||||
IMU'dan bağımsız bir position observation'dır. WLS yalnız geçerli olduğunda
|
||||
ölçüm epoch'unda uygulanır; epoch iki IMU sample'ı arasındaysa input lineer
|
||||
interpolate edilir, update sonrası kalan IMU geçmişi current-time horizon'a
|
||||
replay edilir. Ana döngü IMU kuyruğunu WLS'den önce ve sonra tamamen boşaltır.
|
||||
|
||||
`async_rate_matrix_test.c` aynı üretim C kaynaklarına 52 Hz timestamp'li IMU,
|
||||
100/200/1000 ms GPS, 73/310 ms delivery delay, farklı eksenlerde ivmelenme ve
|
||||
fren, yaw dönüşü ve 10--18 s GPS outage verir. Sonuçlar sırasıyla 0.172/0.180/
|
||||
0.389 m yatay RMSE ve outage sırasında 0.634/0.671/0.822 m tepe hatadır; üçünde
|
||||
de GPS recovery kabul edilmiş, false ZUPT ve NIS reject sıfırdır. Tam GPS'siz
|
||||
koşu da kendine konum feed etmeden yerel dead-reckoning yapar: global origin
|
||||
false kalır, kovaryans büyür ve 25 s sonunda 3.674 m drift oluşur. Bu drift
|
||||
gizlenmez; mutlak konumun IMU-only durumda gözlenememesinin beklenen sonucudur.
|
||||
|
||||
`gps_reacquisition_test.c`, canlı UART'ta görülen kötü ilk GPS fix'i senaryosunu
|
||||
ENU'da 80/40/-170 m başlangıç ofsetiyle tekrarlar. Hard NIS kapısı tek başına
|
||||
sonraki doğru fix'leri sürekli reddettiği için fusion engine zaman tabanlı bir
|
||||
position reacquisition uygular: son başarılı position fusion üzerinden yedi
|
||||
saniye timeout geçtikten ve en az üç rejected fix kendi içinde devamlı olduktan
|
||||
sonra yalnız position state resetlenir. Hız, attitude ve IMU bias'ları korunur.
|
||||
Test 100/200/1000 ms GPS periyotlarında sıfır son hata üretir; ayrı
|
||||
150/-120/+80 m tek-fix testi aykırı ölçümün reddedildiğini ve state reset
|
||||
oluşturmadığını doğrular.
|
||||
|
||||
Testin `--trace` seçeneği UART'a benzer `IMU ts=...`, `GPS ts=... arrival=...`
|
||||
satırları üretir. Standart regression komutu:
|
||||
|
||||
```sh
|
||||
make -C soc/software/fusion/tests test
|
||||
```
|
||||
|
||||
`platform_scenario_matrix_test.c` ayrıca stationary/tilt/vibration, yaya,
|
||||
trafik ışığında duran otomobil ve ani dönüşlü/yüksek ivmeli uçak profillerini;
|
||||
200/500/1000 ms GNSS ile 19/20/200/300 ms IMU'nun 48 kombinasyonunda çalıştırır.
|
||||
GNSS dört uydu kullanır, ilk bölümde 5--10 m, sonra 3--5 m sınıfı yatay gürültü,
|
||||
3 saniye outage ve 2 saniye tutarlı büyük hata içerir. Kısa hata yedi saniyelik
|
||||
recovery timeout'unu açamaz. 200/300 ms IMU yüksek dinamikteki doğruluk kaybıyla
|
||||
birlikte açıkça raporlanır; test bunu daha sık IMU varmış gibi gizlemez.
|
||||
|
||||
```text
|
||||
Offline model:
|
||||
UART GPS + IMU logları -> Python parser -> fixed/adaptive ESKF -> CSV/grafik/harita
|
||||
@@ -213,16 +262,16 @@ yoktur.
|
||||
|
||||
Temel parçalar:
|
||||
|
||||
- `gps_system/src/eskf.c`: ENU konum/hız, body-to-ENU quaternion, accel bias,
|
||||
- `gps_system/src/fusion/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
|
||||
- `gps_system/src/fusion/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,
|
||||
- `gps_system/src/fusion/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
|
||||
- `gps_system/src/fusion/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
|
||||
- `gps_system/src/drivers/imu/tdk_icm42688.c`: ICM-42688-P SPI/INT1 sürücüsü;
|
||||
- `gps_system/src/fusion/navigation_fusion.c`: GPS ve IMU kuyruklarını algoritmaya
|
||||
bağlayan üst koordinatör.
|
||||
|
||||
### ESKF state ve matematik sınırı
|
||||
@@ -270,6 +319,64 @@ Default ESKF process/başlangıç değerleri:
|
||||
| Maksimum propagation alt adımı | 20 ms |
|
||||
| Konum NIS kapısı | 16.266236, 3 DOF %99.9 |
|
||||
|
||||
### Innovation rejection, recovery ve katman siniri (2026-07-16)
|
||||
|
||||
Standart ESKF position update'i tek basina buyuk ve kalici bir baslangic
|
||||
ofsetini duzeltemez. GPS--INS residual'i NIS kapisini astiginda olcum state'e
|
||||
hic uygulanmaz; state GPS'e yaklasmadigi icin sonraki dogru GPS fix'leri de
|
||||
ayni nedenle sonsuza kadar reddedilebilir. NIS yalniz iki bilgi kaynaginin
|
||||
tutarsiz oldugunu soyler. GPS'in mi hatali, yoksa ataletsel cozumun mu yanlis
|
||||
ankrajlandigini matematiksel update tek basina belirleyemez.
|
||||
|
||||
Bu nedenle sorumluluk iki katmana ayrilir:
|
||||
|
||||
- `eskf.c` innovation, innovation covariance, NIS gate, kabul edilmis Kalman
|
||||
correction ve covariance bakimindan dogru position-reset primitive'ini
|
||||
icerir. Rejected olcumu gizlice correction'a sokmaz ve NIS'i bypass etmez.
|
||||
- `fusion_engine.c` sensor gecmisi ve zaman bilgisine sahip supervisor'dur.
|
||||
Ardisik rejected GNSS fix'lerinin kendi aralarinda zamansal olarak tutarli
|
||||
olup olmadigini izler ve gerekli recovery/reset kararini burada verir.
|
||||
|
||||
Mevcut yalın recovery, son başarılı position fusion'dan sonra yedi saniyelik
|
||||
timeout dolmuşsa ve en az üç rejected position fix'i kendi arasında zamansal
|
||||
tutarlıysa yalnız position state ve position covariance'ini GNSS'e yeniden
|
||||
ankrajlar. Position cross-covariance'lari gecersiz korelasyon tasimamasi icin
|
||||
sifirlanir; velocity, attitude ve IMU bias state'leri korunur. Hard NIS gate
|
||||
aktif kalir ve tek aykiri olcum reset uretemez. Bu bir innovation-bypass
|
||||
workaround'u degil, kontrollu state recovery mekanizmasidir.
|
||||
|
||||
PX4 EKF2 de ayni temel ayrimi yapar. Generic `fuseHorizontalPosition()` yalniz
|
||||
innovation testinden gecen olcumu fuse eder. GNSS'e baslama, quality check,
|
||||
`time_last_fuse` timeout'u, state reset, fusion stop ve sensor-fault kararlari
|
||||
ayri GNSS control akisindadir. Otomatik modda baska yatay konum kaynagi yoksa
|
||||
uzun fusion timeout'undan sonra GNSS'e reset yapilabilir; alternatif aiding
|
||||
varsa veya dead-reckoning politikasinda GNSS durdurulabilir/fault kabul
|
||||
edilebilir. Kaynaklar:
|
||||
|
||||
- <https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp>
|
||||
- <https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/ekf2/EKF/position_fusion.cpp>
|
||||
- <https://docs.px4.io/main/en/advanced_config/tuning_the_ecl_ekf>
|
||||
|
||||
Mevcut recovery PX4'teki aiding yoneticisinin sade bare-metal karsiligidir;
|
||||
nihai mimari hedefi asagidaki acik GNSS aiding durum makinesidir:
|
||||
|
||||
```text
|
||||
DISABLED -> STARTING -> FUSING -> FAULTED/RECOVERING
|
||||
```
|
||||
|
||||
Karar yalniz rejected-candidate suresine degil son basarili position fusion
|
||||
zamanina baglanmalidir. Uydu sayisi, finite covariance, DOP, WLS integrity ve
|
||||
alternatif aiding kaynaklari birlikte degerlendirilmelidir. Tam dort uyduda
|
||||
WLS residual redundancy/chi-square DOF bulunmadigi icin temporal consistency
|
||||
daha onemlidir. Ileride Doppler velocity ayri innovation, fusion timeout ve
|
||||
velocity-reset politikasi ile fuse edilmelidir. Yatay position ve height da
|
||||
barometre gibi ek kaynaklar geldiginde birbirinden bagimsiz yonetilmelidir.
|
||||
|
||||
Baslangic GNSS'i icin mutlak bir dogruluk referansi yoksa tutarli fakat yanlis
|
||||
bir GNSS kumesini otomatik olarak gercek konumdan ayirmak temel olarak mumkun
|
||||
degildir. Kisa bir stable-fix/startup-quality penceresi riski azaltir, fakat
|
||||
spoofing veya tutarli multipath'e karsi tek basina kanit degildir.
|
||||
|
||||
### Asenkron veri akışı ve timestamp
|
||||
|
||||
IMU ve GPS aynı local monotonic CPU/CLINT tick alanında timestamp edilir.
|
||||
@@ -313,6 +420,13 @@ verir. Yaw gravity ile gözlenemediği için başlangıçta sıfır referanslı
|
||||
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.
|
||||
|
||||
IMU motion-state eşikleri tanılama için tutulur, fakat ZUPT yetkisini tek başına
|
||||
vermez. PX4 ayrımına uygun olarak `fusion_rest_detector` titreşim metriklerini
|
||||
izler ve dışarıdan gelen `platform_landed` kapısıyla birleşir. Prototipin unarmed
|
||||
bench modu landed=true başlar; gerçek araç kontrolü bu değeri kendi state'inden
|
||||
güncellemelidir. Sessiz IMU, landed=false iken sabit hızlı hareketi stationary
|
||||
ilan edemez.
|
||||
|
||||
Motion-state default eşikleri:
|
||||
|
||||
| Karar parametresi | Değer |
|
||||
@@ -328,12 +442,20 @@ Motion-state default eşikleri:
|
||||
| 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.
|
||||
Güçlü accel/gyro/jerk veya yüksek ESKF hızı motion tanılamasını tek örnekte
|
||||
moving'e çıkarabilir. ZUPT ve zero-gyro ise yalnız ardışık iki timestamp'li
|
||||
örnekte dış supervisor `vehicle_at_rest=true` verdiğinde 200 ms periyotla
|
||||
uygulanır. ZUPT velocity sigma 0.20 m/s'dir. Zero-gyro aynı 200 ms penceredeki
|
||||
delta-angle toplamından ortalama açısal hız üretir; tek bir örneği bias diye
|
||||
kabul etmez. Bu politika PX4 EKF2 `ZeroVelocityUpdate`/`ZeroGyroUpdate` ve land
|
||||
detector katman ayrımının bare-metal karşılığıdır.
|
||||
|
||||
Kaynaklar:
|
||||
|
||||
- <https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp>
|
||||
- <https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp>
|
||||
- <https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/land_detector/LandDetector.cpp>
|
||||
- <https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/sensors/vehicle_imu/VehicleIMU.cpp>
|
||||
|
||||
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
|
||||
@@ -836,7 +958,7 @@ azınlık TOW'unu çoğunluğa kopyalayan geçici recovery yolu da kaldırıldı
|
||||
veya problemli kanal reject edilir. TOW değeri sessizce başka kanalın
|
||||
değeriyle değiştirilmez.
|
||||
|
||||
Üretim kodu `header/gps_time_anchor.h` ve `src/gps_time_anchor.c` içindedir.
|
||||
Üretim kodu `header/gps/gps_time_anchor.h` ve `src/gps/gps_time_anchor.c` içindedir.
|
||||
`parse_subframe()` artık TOW state'ini değiştirmez; yalnız HOW TOW değerini
|
||||
dışarı verir. `gps_time_from_sample_anchor()` her nav-solve için GPS zamanını
|
||||
son HOW ve iki receiver-sample latch'i arasındaki farktan baştan üretir.
|
||||
|
||||
@@ -3,13 +3,13 @@ CFLAGS ?= -std=c11 -O2 -Wall -Wextra -Wpedantic
|
||||
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
|
||||
IMU_PROFILE_SOURCE := ../../gps_system/src/imu_sensor_profile.c
|
||||
GPS_TIME_SOURCE := ../../gps_system/src/gps/gps_time_anchor.c
|
||||
ESKF_SOURCES := ../../gps_system/src/fusion/eskf.c ../../gps_system/src/fusion/fusion_motion.c ../../gps_system/src/fusion/fusion_rest_detector.c ../../gps_system/src/fusion/fusion_engine.c ../../gps_system/src/fusion/fusion_gps_adapter.c ../../gps_system/src/fusion/fusion_bus.c
|
||||
IMU_PROFILE_SOURCE := ../../gps_system/src/adapters/imu_fusion_profile.c
|
||||
|
||||
.PHONY: all test sanitize profile benchmark clean
|
||||
.PHONY: all test scenario-logs sanitize profile benchmark clean
|
||||
|
||||
all: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test imu_quality_synthetic_test
|
||||
all: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test
|
||||
|
||||
standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
@@ -17,19 +17,58 @@ standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE
|
||||
eskf_motion_test: eskf_motion_test.c $(ESKF_SOURCES)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
fusion_rest_detector_test: fusion_rest_detector_test.c ../../gps_system/src/fusion/fusion_rest_detector.c
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
fusion_synthetic_test: fusion_synthetic_test.c $(ESKF_SOURCES)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
imu_quality_synthetic_test: imu_quality_synthetic_test.c $(ESKF_SOURCES) $(IMU_PROFILE_SOURCE)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
test: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test imu_quality_synthetic_test
|
||||
async_tilt_recovery_test: async_tilt_recovery_test.c $(ESKF_SOURCES)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
async_rate_matrix_test: async_rate_matrix_test.c $(ESKF_SOURCES)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
gps_reacquisition_test: gps_reacquisition_test.c $(ESKF_SOURCES)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
platform_scenario_matrix_test: platform_scenario_matrix_test.c $(ESKF_SOURCES)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test
|
||||
./standalone_gps_integrity_test
|
||||
./eskf_motion_test
|
||||
./fusion_rest_detector_test
|
||||
./fusion_synthetic_test
|
||||
./imu_quality_synthetic_test
|
||||
./async_tilt_recovery_test
|
||||
./async_rate_matrix_test
|
||||
./gps_reacquisition_test
|
||||
./platform_scenario_matrix_test
|
||||
|
||||
scenario-logs: platform_scenario_matrix_test
|
||||
mkdir -p output/scenarios
|
||||
@for scenario in stationary pedestrian car aircraft; do \
|
||||
for gps in 200 500 1000; do \
|
||||
for imu in 19 20 200 300; do \
|
||||
./platform_scenario_matrix_test --log $$scenario $$gps $$imu \
|
||||
output/scenarios/$${scenario}_gps$${gps}_imu$${imu}.log >/dev/null || exit $$?; \
|
||||
done; \
|
||||
done; \
|
||||
done
|
||||
@echo "Wrote 48 replay logs under output/scenarios/"
|
||||
|
||||
sanitize:
|
||||
$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
|
||||
-fsanitize=address,undefined -fno-omit-frame-pointer \
|
||||
-I../../gps_system/header fusion_rest_detector_test.c \
|
||||
../../gps_system/src/fusion/fusion_rest_detector.c \
|
||||
-o fusion_rest_detector_test_san $(LDLIBS)
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 ./fusion_rest_detector_test_san
|
||||
$(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) \
|
||||
@@ -48,6 +87,30 @@ sanitize:
|
||||
$(IMU_PROFILE_SOURCE) -o imu_quality_synthetic_test_san $(LDLIBS)
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 ./imu_quality_synthetic_test_san
|
||||
$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
|
||||
-fsanitize=address,undefined -fno-omit-frame-pointer \
|
||||
-I../../gps_system/header async_tilt_recovery_test.c $(ESKF_SOURCES) \
|
||||
-o async_tilt_recovery_test_san $(LDLIBS)
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 ./async_tilt_recovery_test_san
|
||||
$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
|
||||
-fsanitize=address,undefined -fno-omit-frame-pointer \
|
||||
-I../../gps_system/header async_rate_matrix_test.c $(ESKF_SOURCES) \
|
||||
-o async_rate_matrix_test_san $(LDLIBS)
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 ./async_rate_matrix_test_san
|
||||
$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
|
||||
-fsanitize=address,undefined -fno-omit-frame-pointer \
|
||||
-I../../gps_system/header gps_reacquisition_test.c $(ESKF_SOURCES) \
|
||||
-o gps_reacquisition_test_san $(LDLIBS)
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 ./gps_reacquisition_test_san
|
||||
$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
|
||||
-fsanitize=address,undefined -fno-omit-frame-pointer \
|
||||
-I../../gps_system/header platform_scenario_matrix_test.c $(ESKF_SOURCES) \
|
||||
-o platform_scenario_matrix_test_san $(LDLIBS)
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 ./platform_scenario_matrix_test_san
|
||||
|
||||
profile:
|
||||
$(CC) -std=c11 -O2 -g -pg -Wall -Wextra -Wpedantic \
|
||||
@@ -62,4 +125,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 eskf_motion_test eskf_motion_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace imu_quality_synthetic_test imu_quality_synthetic_test_san 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_rest_detector_test fusion_rest_detector_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace imu_quality_synthetic_test imu_quality_synthetic_test_san async_tilt_recovery_test async_tilt_recovery_test_san async_rate_matrix_test async_rate_matrix_test_san gps_reacquisition_test gps_reacquisition_test_san platform_scenario_matrix_test platform_scenario_matrix_test_san gmon.out
|
||||
|
||||
@@ -0,0 +1,416 @@
|
||||
#include "fusion/fusion_engine.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#define TIMEBASE_HZ 1000000u
|
||||
#define IMU_HZ 52u
|
||||
#define DURATION_S 25.0
|
||||
#define PI 3.14159265358979323846264338327950288
|
||||
#define GPS_EVENT_CAPACITY 300u
|
||||
|
||||
typedef struct {
|
||||
double position_m[3];
|
||||
double velocity_mps[3];
|
||||
double yaw_rad;
|
||||
double accel_nav_mps2[3];
|
||||
double yaw_rate_rps;
|
||||
} truth_t;
|
||||
|
||||
typedef struct {
|
||||
fusion_position_measurement_t measurement;
|
||||
uint64_t arrival_ticks;
|
||||
} gps_event_t;
|
||||
|
||||
typedef struct {
|
||||
unsigned int gps_period_ms;
|
||||
double horizontal_rmse_m;
|
||||
double peak_outage_error_m;
|
||||
double final_horizontal_error_m;
|
||||
double final_vertical_error_m;
|
||||
double final_speed_mps;
|
||||
double outage_sigma_growth_m;
|
||||
double final_horizontal_sigma_m;
|
||||
double maximum_position_step_m;
|
||||
uint64_t gps_accepted;
|
||||
uint64_t gps_rejected;
|
||||
uint64_t replay_count;
|
||||
uint64_t imu_received;
|
||||
unsigned int false_stationary_samples;
|
||||
bool moving_detected;
|
||||
bool recovery_gps_accepted;
|
||||
bool local_state_finite;
|
||||
bool covariance_finite_positive;
|
||||
bool global_origin_available;
|
||||
} metrics_t;
|
||||
|
||||
static bool trace_enabled;
|
||||
|
||||
static double square(double value)
|
||||
{
|
||||
return value * value;
|
||||
}
|
||||
|
||||
static void command_at(double time_s, double accel[3], double* yaw_rate)
|
||||
{
|
||||
memset(accel, 0, 3u * sizeof(double));
|
||||
*yaw_rate = 0.0;
|
||||
if((time_s >= 3.0) && (time_s < 5.0))
|
||||
{
|
||||
accel[0] = 1.0;
|
||||
}
|
||||
else if((time_s >= 8.0) && (time_s < 10.0))
|
||||
{
|
||||
accel[0] = -1.0;
|
||||
}
|
||||
else if((time_s >= 12.0) && (time_s < 14.0))
|
||||
{
|
||||
accel[1] = 0.5;
|
||||
*yaw_rate = PI / 4.0;
|
||||
}
|
||||
else if((time_s >= 18.0) && (time_s < 20.0))
|
||||
{
|
||||
accel[1] = -0.5;
|
||||
}
|
||||
}
|
||||
|
||||
static truth_t truth_at(double time_s)
|
||||
{
|
||||
static const double boundaries[] = {
|
||||
0.0, 3.0, 5.0, 8.0, 10.0, 12.0, 14.0, 18.0, 20.0, DURATION_S
|
||||
};
|
||||
truth_t truth;
|
||||
memset(&truth, 0, sizeof(truth));
|
||||
for(unsigned int segment = 0u;
|
||||
segment + 1u < sizeof(boundaries) / sizeof(boundaries[0]); segment++)
|
||||
{
|
||||
double start = boundaries[segment];
|
||||
if(time_s <= start)
|
||||
{
|
||||
break;
|
||||
}
|
||||
double end = fmin(time_s, boundaries[segment + 1u]);
|
||||
double dt = end - start;
|
||||
double accel[3];
|
||||
double yaw_rate;
|
||||
command_at(start + 0.5 * dt, accel, &yaw_rate);
|
||||
for(unsigned int axis = 0u; axis < 3u; axis++)
|
||||
{
|
||||
truth.position_m[axis] += truth.velocity_mps[axis] * dt +
|
||||
0.5 * accel[axis] * dt * dt;
|
||||
truth.velocity_mps[axis] += accel[axis] * dt;
|
||||
}
|
||||
truth.yaw_rad += yaw_rate * dt;
|
||||
if(end >= time_s)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
command_at(time_s, truth.accel_nav_mps2, &truth.yaw_rate_rps);
|
||||
return truth;
|
||||
}
|
||||
|
||||
static fusion_imu_measurement_t make_imu(uint64_t ticks, double gravity)
|
||||
{
|
||||
double time_s = (double)ticks / (double)TIMEBASE_HZ;
|
||||
truth_t truth = truth_at(time_s);
|
||||
double cosine = cos(truth.yaw_rad);
|
||||
double sine = sin(truth.yaw_rad);
|
||||
double specific_force_nav[3] = {
|
||||
truth.accel_nav_mps2[0], truth.accel_nav_mps2[1],
|
||||
truth.accel_nav_mps2[2] + gravity
|
||||
};
|
||||
fusion_imu_measurement_t imu;
|
||||
memset(&imu, 0, sizeof(imu));
|
||||
imu.timestamp_ticks = ticks;
|
||||
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] = truth.yaw_rate_rps;
|
||||
/* Constant offsets are learned during stationary startup. Small
|
||||
* deterministic broadband components keep the test reproducible. */
|
||||
static const double accel_bias[3] = {0.030, -0.020, 0.050};
|
||||
static const double gyro_bias[3] = {0.0008, -0.0005, 0.0006};
|
||||
for(unsigned int axis = 0u; axis < 3u; axis++)
|
||||
{
|
||||
double phase = time_s * (11.0 + 2.0 * axis) + (double)axis;
|
||||
imu.accel_mps2[axis] += accel_bias[axis] + 0.008 * sin(phase);
|
||||
imu.gyro_rps[axis] += gyro_bias[axis] + 0.00025 * cos(1.7 * phase);
|
||||
}
|
||||
imu.temperature_c = 25.0 + 0.4 * sin(0.1 * time_s);
|
||||
imu.sequence = (uint32_t)((ticks * IMU_HZ + TIMEBASE_HZ / 2u) /
|
||||
TIMEBASE_HZ);
|
||||
imu.vehicle_at_rest =
|
||||
(hypot(truth.velocity_mps[0], truth.velocity_mps[1]) < 0.05) &&
|
||||
(hypot(truth.accel_nav_mps2[0], truth.accel_nav_mps2[1]) < 0.05) &&
|
||||
(fabs(truth.yaw_rate_rps) < 0.02);
|
||||
return imu;
|
||||
}
|
||||
|
||||
static fusion_position_measurement_t make_gps(uint64_t ticks)
|
||||
{
|
||||
double time_s = (double)ticks / (double)TIMEBASE_HZ;
|
||||
truth_t truth = truth_at(time_s);
|
||||
fusion_position_measurement_t gps;
|
||||
memset(&gps, 0, sizeof(gps));
|
||||
gps.timestamp_ticks = ticks;
|
||||
gps.position_enu_m[0] = truth.position_m[0] + 0.45 * sin(7.1 * time_s);
|
||||
gps.position_enu_m[1] = truth.position_m[1] + 0.45 * cos(5.3 * time_s);
|
||||
gps.position_enu_m[2] = truth.position_m[2] + 0.70 * sin(3.7 * time_s);
|
||||
gps.covariance_enu_m2[0] = square(1.2);
|
||||
gps.covariance_enu_m2[4] = square(1.2);
|
||||
gps.covariance_enu_m2[8] = square(2.0);
|
||||
gps.source_quality = 1.0;
|
||||
gps.source_valid = true;
|
||||
return gps;
|
||||
}
|
||||
|
||||
static bool covariance_diagonal_valid(const eskf_t* filter)
|
||||
{
|
||||
for(unsigned int index = 0u; index < ESKF_ERROR_STATE_DIM; index++)
|
||||
{
|
||||
if(!isfinite(filter->covariance[index][index]) ||
|
||||
!(filter->covariance[index][index] > 0.0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static metrics_t run_scenario(unsigned int gps_period_ms)
|
||||
{
|
||||
static fusion_engine_t engine;
|
||||
memset(&engine, 0, sizeof(engine));
|
||||
fusion_engine_config_t config;
|
||||
fusion_engine_default_config(&config, TIMEBASE_HZ);
|
||||
config.calibration_min_samples = 100u;
|
||||
metrics_t metrics;
|
||||
memset(&metrics, 0, sizeof(metrics));
|
||||
metrics.gps_period_ms = gps_period_ms;
|
||||
if(!fusion_engine_init(&engine, &config))
|
||||
{
|
||||
return metrics;
|
||||
}
|
||||
if(gps_period_ms != 0u)
|
||||
{
|
||||
fusion_position_measurement_t origin = make_gps(0u);
|
||||
origin.position_enu_m[0] = 0.0;
|
||||
origin.position_enu_m[1] = 0.0;
|
||||
origin.position_enu_m[2] = 0.0;
|
||||
(void)fusion_engine_push_position(&engine, &origin, NULL, NULL);
|
||||
}
|
||||
|
||||
gps_event_t events[GPS_EVENT_CAPACITY];
|
||||
unsigned int event_head = 0u;
|
||||
unsigned int event_count = 0u;
|
||||
uint64_t next_gps_ticks = 3037000u;
|
||||
uint64_t gps_period_ticks = (uint64_t)gps_period_ms * 1000u;
|
||||
double error_square_sum = 0.0;
|
||||
uint64_t error_samples = 0u;
|
||||
double sigma_at_outage_start = NAN;
|
||||
double sigma_before_recovery = NAN;
|
||||
uint64_t accepted_before_recovery = 0u;
|
||||
double previous_position[3] = {0.0, 0.0, 0.0};
|
||||
bool have_previous_position = false;
|
||||
uint64_t sample_count = (uint64_t)(DURATION_S * (double)IMU_HZ);
|
||||
|
||||
for(uint64_t sample = 0u; sample <= sample_count; sample++)
|
||||
{
|
||||
uint64_t ticks = (sample * TIMEBASE_HZ + IMU_HZ / 2u) / IMU_HZ;
|
||||
double time_s = (double)ticks / (double)TIMEBASE_HZ;
|
||||
fusion_imu_measurement_t imu = make_imu(ticks, config.eskf.gravity_mps2);
|
||||
if(gps_period_ms == 0u)
|
||||
{
|
||||
/* Preserve the pure unaided observability case: no GNSS and no
|
||||
* external platform-rest constraint. */
|
||||
imu.vehicle_at_rest = false;
|
||||
}
|
||||
if(trace_enabled)
|
||||
{
|
||||
printf("IMU ts=%llu seq=%u accel=%.6f,%.6f,%.6f gyro=%.6f,%.6f,%.6f\n",
|
||||
(unsigned long long)imu.timestamp_ticks, imu.sequence,
|
||||
imu.accel_mps2[0], imu.accel_mps2[1], imu.accel_mps2[2],
|
||||
imu.gyro_rps[0], imu.gyro_rps[1], imu.gyro_rps[2]);
|
||||
}
|
||||
if(!fusion_engine_push_imu(&engine, &imu))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if((gps_period_ms != 0u) && (ticks >= next_gps_ticks))
|
||||
{
|
||||
double gps_time_s = (double)next_gps_ticks / (double)TIMEBASE_HZ;
|
||||
if(!((gps_time_s >= 10.0) && (gps_time_s < 18.0)) &&
|
||||
(event_count < GPS_EVENT_CAPACITY))
|
||||
{
|
||||
events[event_count].measurement = make_gps(next_gps_ticks);
|
||||
events[event_count].arrival_ticks = next_gps_ticks +
|
||||
((gps_period_ms == 1000u) ? 310000u : 73000u);
|
||||
event_count++;
|
||||
}
|
||||
next_gps_ticks += gps_period_ticks;
|
||||
}
|
||||
|
||||
while((event_head < event_count) &&
|
||||
(events[event_head].arrival_ticks <= ticks))
|
||||
{
|
||||
eskf_update_result_t result = ESKF_UPDATE_INVALID;
|
||||
double nis = NAN;
|
||||
(void)fusion_engine_push_position(
|
||||
&engine, &events[event_head].measurement, &result, &nis);
|
||||
if(trace_enabled)
|
||||
{
|
||||
printf("GPS ts=%llu arrival=%llu result=%u nis=%.6f pos=%.3f,%.3f,%.3f\n",
|
||||
(unsigned long long)events[event_head].measurement.timestamp_ticks,
|
||||
(unsigned long long)ticks, (unsigned int)result, nis,
|
||||
events[event_head].measurement.position_enu_m[0],
|
||||
events[event_head].measurement.position_enu_m[1],
|
||||
events[event_head].measurement.position_enu_m[2]);
|
||||
}
|
||||
event_head++;
|
||||
}
|
||||
|
||||
if(engine.state == FUSION_ENGINE_RUNNING)
|
||||
{
|
||||
truth_t truth = truth_at(time_s);
|
||||
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 horizontal_error = hypot(east_error, north_error);
|
||||
if(have_previous_position)
|
||||
{
|
||||
double position_step = hypot(
|
||||
engine.current.filter.position_m[0] - previous_position[0],
|
||||
engine.current.filter.position_m[1] - previous_position[1]);
|
||||
metrics.maximum_position_step_m =
|
||||
fmax(metrics.maximum_position_step_m, position_step);
|
||||
}
|
||||
memcpy(previous_position, engine.current.filter.position_m,
|
||||
sizeof(previous_position));
|
||||
have_previous_position = true;
|
||||
if(time_s >= 2.5)
|
||||
{
|
||||
error_square_sum += square(horizontal_error);
|
||||
error_samples++;
|
||||
}
|
||||
if((time_s >= 10.0) && (time_s < 18.0))
|
||||
{
|
||||
metrics.peak_outage_error_m =
|
||||
fmax(metrics.peak_outage_error_m, horizontal_error);
|
||||
}
|
||||
if((hypot(truth.velocity_mps[0], truth.velocity_mps[1]) > 0.2) &&
|
||||
(engine.current.motion.state == FUSION_MOTION_STATIONARY))
|
||||
{
|
||||
metrics.false_stationary_samples++;
|
||||
}
|
||||
if(engine.current.motion.state == FUSION_MOTION_MOVING)
|
||||
{
|
||||
metrics.moving_detected = true;
|
||||
}
|
||||
if(isnan(sigma_at_outage_start) && (time_s >= 10.0))
|
||||
{
|
||||
sigma_at_outage_start = sqrt(engine.current.filter.covariance[0][0]);
|
||||
accepted_before_recovery = engine.diagnostics.position_accepted;
|
||||
}
|
||||
if(isnan(sigma_before_recovery) && (time_s >= 18.0))
|
||||
{
|
||||
sigma_before_recovery = sqrt(engine.current.filter.covariance[0][0]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
truth_t final_truth = truth_at(DURATION_S);
|
||||
metrics.horizontal_rmse_m = (error_samples != 0u)
|
||||
? sqrt(error_square_sum / (double)error_samples) : INFINITY;
|
||||
metrics.final_horizontal_error_m = hypot(
|
||||
engine.current.filter.position_m[0] - final_truth.position_m[0],
|
||||
engine.current.filter.position_m[1] - final_truth.position_m[1]);
|
||||
metrics.final_vertical_error_m =
|
||||
fabs(engine.current.filter.position_m[2] - final_truth.position_m[2]);
|
||||
metrics.final_speed_mps = eskf_speed_mps(&engine.current.filter);
|
||||
metrics.outage_sigma_growth_m =
|
||||
(isfinite(sigma_at_outage_start) && isfinite(sigma_before_recovery))
|
||||
? sigma_before_recovery - sigma_at_outage_start : 0.0;
|
||||
metrics.final_horizontal_sigma_m = sqrt(
|
||||
engine.current.filter.covariance[0][0] +
|
||||
engine.current.filter.covariance[1][1]);
|
||||
metrics.gps_accepted = engine.diagnostics.position_accepted;
|
||||
metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
|
||||
metrics.replay_count = engine.diagnostics.replay_count;
|
||||
metrics.imu_received = engine.diagnostics.imu_received;
|
||||
metrics.recovery_gps_accepted = (gps_period_ms == 0u) ||
|
||||
(engine.diagnostics.position_accepted > accepted_before_recovery);
|
||||
metrics.local_state_finite = (engine.state == FUSION_ENGINE_RUNNING) &&
|
||||
eskf_state_is_finite(&engine.current.filter);
|
||||
metrics.covariance_finite_positive =
|
||||
covariance_diagonal_valid(&engine.current.filter);
|
||||
metrics.global_origin_available = engine.have_initial_position;
|
||||
return metrics;
|
||||
}
|
||||
|
||||
static void print_metrics(const metrics_t* metrics)
|
||||
{
|
||||
printf("GPS=%4u ms IMU=%llu GPS-ok/rej=%llu/%llu replay=%llu "
|
||||
"rmse-h=%.3f m outage-peak=%.3f m sigma-growth=%.3f m "
|
||||
"final=%.3f/%.3f m sigma-h=%.3f m step-max=%.3f m "
|
||||
"speed=%.4f m/s false-ZUPT=%u origin=%u\n",
|
||||
metrics->gps_period_ms,
|
||||
(unsigned long long)metrics->imu_received,
|
||||
(unsigned long long)metrics->gps_accepted,
|
||||
(unsigned long long)metrics->gps_rejected,
|
||||
(unsigned long long)metrics->replay_count,
|
||||
metrics->horizontal_rmse_m, metrics->peak_outage_error_m,
|
||||
metrics->outage_sigma_growth_m,
|
||||
metrics->final_horizontal_error_m, metrics->final_vertical_error_m,
|
||||
metrics->final_horizontal_sigma_m, metrics->maximum_position_step_m,
|
||||
metrics->final_speed_mps, metrics->false_stationary_samples,
|
||||
metrics->global_origin_available ? 1u : 0u);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
trace_enabled = (argc == 2) && (strcmp(argv[1], "--trace") == 0);
|
||||
const unsigned int periods_ms[] = {100u, 200u, 1000u, 0u};
|
||||
bool passed = true;
|
||||
printf("Timestamped 52 Hz IMU, asynchronous GPS, 8 s outage (10..18 s)\n");
|
||||
for(unsigned int index = 0u;
|
||||
index < sizeof(periods_ms) / sizeof(periods_ms[0]); index++)
|
||||
{
|
||||
metrics_t metrics = run_scenario(periods_ms[index]);
|
||||
print_metrics(&metrics);
|
||||
bool common_ok = metrics.local_state_finite &&
|
||||
metrics.covariance_finite_positive &&
|
||||
metrics.moving_detected &&
|
||||
(metrics.false_stationary_samples == 0u) &&
|
||||
(metrics.imu_received == 1301u) &&
|
||||
(metrics.maximum_position_step_m < 1.0);
|
||||
bool gps_ok = (periods_ms[index] == 0u)
|
||||
? (!metrics.global_origin_available && (metrics.gps_accepted == 0u) &&
|
||||
(metrics.peak_outage_error_m < 1.5) &&
|
||||
(metrics.final_horizontal_error_m < 5.0) &&
|
||||
(metrics.final_horizontal_error_m <
|
||||
3.0 * metrics.final_horizontal_sigma_m) &&
|
||||
(metrics.outage_sigma_growth_m > 0.0))
|
||||
: (metrics.global_origin_available && metrics.recovery_gps_accepted &&
|
||||
(metrics.gps_rejected == 0u) && (metrics.replay_count > 0u) &&
|
||||
(metrics.peak_outage_error_m < 2.0) &&
|
||||
(metrics.outage_sigma_growth_m > 0.0) &&
|
||||
(metrics.final_speed_mps < 0.08) &&
|
||||
(metrics.final_horizontal_error_m < 1.5) &&
|
||||
(metrics.final_vertical_error_m < 1.5));
|
||||
passed = passed && common_ok && gps_ok;
|
||||
}
|
||||
if(!passed)
|
||||
{
|
||||
fprintf(stderr, "FAIL: asynchronous rate/dead-reckoning matrix.\n");
|
||||
return 1;
|
||||
}
|
||||
printf("PASS: all rates use the same IMU prediction path; GPS loss remains "
|
||||
"local dead reckoning and recovery is an external update.\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,315 @@
|
||||
#include "fusion/fusion_engine.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#define TEST_TIMEBASE_HZ 1000000u
|
||||
#define TEST_IMU_HZ 52u
|
||||
#define TEST_DURATION_S 30u
|
||||
#define TEST_PI 3.14159265358979323846264338327950288
|
||||
#define GPS_PERIOD_TICKS 200000u
|
||||
#define GPS_DELAY_TICKS 73000u
|
||||
#define MAX_GPS_EVENTS 160u
|
||||
|
||||
typedef struct {
|
||||
fusion_position_measurement_t measurement;
|
||||
uint64_t arrival_ticks;
|
||||
} gps_event_t;
|
||||
|
||||
typedef struct {
|
||||
const char* name;
|
||||
double peak_horizontal_error_m;
|
||||
double peak_vertical_error_m;
|
||||
double final_horizontal_error_m;
|
||||
double final_vertical_error_m;
|
||||
double final_speed_mps;
|
||||
double final_roll_error_deg;
|
||||
uint64_t imu_pushed;
|
||||
uint64_t gps_accepted;
|
||||
uint64_t gps_rejected;
|
||||
uint64_t zero_updates;
|
||||
fusion_engine_state_t engine_state;
|
||||
fusion_motion_state_t motion_state;
|
||||
} scenario_metrics_t;
|
||||
|
||||
static double radians(double degrees)
|
||||
{
|
||||
return degrees * TEST_PI / 180.0;
|
||||
}
|
||||
|
||||
static double degrees(double radians_value)
|
||||
{
|
||||
return radians_value * 180.0 / TEST_PI;
|
||||
}
|
||||
|
||||
/* The IMU rotates about its own X axis without translating. Calibration is
|
||||
* complete before the manoeuvre. This is the smallest synthetic version of
|
||||
* the live hand-tilt failure: the true position and the GNSS antenna remain at
|
||||
* the origin while body-frame gravity changes with roll. */
|
||||
static void truth_roll(uint64_t ticks,
|
||||
double final_roll_rad,
|
||||
double* roll_rad,
|
||||
double* roll_rate_rps)
|
||||
{
|
||||
double time_s = (double)ticks / (double)TEST_TIMEBASE_HZ;
|
||||
double roll = 0.0;
|
||||
double rate = 0.0;
|
||||
const double maximum_roll = radians(20.0);
|
||||
/* A short hand motion is intentionally placed between the 250 ms selected
|
||||
* samples. Full-rate IMU observes the angular rate; sample dropping can
|
||||
* observe only the changed gravity vector and completely miss the gyro. */
|
||||
if((time_s >= 16.05) && (time_s < 16.15))
|
||||
{
|
||||
roll = maximum_roll * (time_s - 16.05) / 0.10;
|
||||
rate = maximum_roll / 0.10;
|
||||
}
|
||||
else if((time_s >= 16.15) && (time_s < 17.05))
|
||||
{
|
||||
roll = maximum_roll;
|
||||
}
|
||||
else if((time_s >= 17.05) && (time_s < 17.15))
|
||||
{
|
||||
double fraction = (time_s - 17.05) / 0.10;
|
||||
roll = maximum_roll + fraction * (final_roll_rad - maximum_roll);
|
||||
rate = (final_roll_rad - maximum_roll) / 0.10;
|
||||
}
|
||||
else if(time_s >= 17.15)
|
||||
{
|
||||
roll = final_roll_rad;
|
||||
}
|
||||
*roll_rad = roll;
|
||||
*roll_rate_rps = rate;
|
||||
}
|
||||
|
||||
static fusion_imu_measurement_t make_imu(uint64_t ticks,
|
||||
double gravity_mps2,
|
||||
double final_roll_rad,
|
||||
double gyro_scale)
|
||||
{
|
||||
double roll;
|
||||
double roll_rate;
|
||||
truth_roll(ticks, final_roll_rad, &roll, &roll_rate);
|
||||
fusion_imu_measurement_t imu;
|
||||
memset(&imu, 0, sizeof(imu));
|
||||
imu.timestamp_ticks = ticks;
|
||||
/* R_nb is a positive X rotation. A stationary accelerometer measures
|
||||
* R_nb^T [0, 0, g], hence [0, g sin(phi), g cos(phi)] in body axes. */
|
||||
imu.accel_mps2[1] = gravity_mps2 * sin(roll);
|
||||
imu.accel_mps2[2] = gravity_mps2 * cos(roll);
|
||||
imu.gyro_rps[0] = gyro_scale * roll_rate;
|
||||
imu.temperature_c = 25.0;
|
||||
imu.sequence = (uint32_t)((ticks * TEST_IMU_HZ + TEST_TIMEBASE_HZ / 2u) /
|
||||
TEST_TIMEBASE_HZ);
|
||||
double time_s = (double)ticks / (double)TEST_TIMEBASE_HZ;
|
||||
imu.vehicle_at_rest = (time_s < 16.05) || (time_s >= 18.15);
|
||||
return imu;
|
||||
}
|
||||
|
||||
static fusion_position_measurement_t make_gps(uint64_t ticks)
|
||||
{
|
||||
fusion_position_measurement_t gps;
|
||||
memset(&gps, 0, sizeof(gps));
|
||||
gps.timestamp_ticks = ticks;
|
||||
gps.covariance_enu_m2[0] = 4.0;
|
||||
gps.covariance_enu_m2[4] = 4.0;
|
||||
gps.covariance_enu_m2[8] = 9.0;
|
||||
gps.source_quality = 100.0;
|
||||
gps.source_valid = true;
|
||||
return gps;
|
||||
}
|
||||
|
||||
static unsigned int build_gps_events(gps_event_t events[MAX_GPS_EVENTS])
|
||||
{
|
||||
unsigned int count = 0u;
|
||||
/* Deliberately offset GNSS epochs from both the 52 Hz IMU samples and
|
||||
* integer seconds. */
|
||||
for(uint64_t ticks = 123457u;
|
||||
ticks <= (uint64_t)TEST_DURATION_S * TEST_TIMEBASE_HZ;
|
||||
ticks += GPS_PERIOD_TICKS)
|
||||
{
|
||||
events[count].measurement = make_gps(ticks);
|
||||
events[count].arrival_ticks = ticks + GPS_DELAY_TICKS;
|
||||
count++;
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
static double quaternion_roll(const double quaternion[4])
|
||||
{
|
||||
double w = quaternion[0];
|
||||
double x = quaternion[1];
|
||||
double y = quaternion[2];
|
||||
double z = quaternion[3];
|
||||
return atan2(2.0 * (w * x + y * z),
|
||||
1.0 - 2.0 * (x * x + y * y));
|
||||
}
|
||||
|
||||
static scenario_metrics_t run_scenario(const char* name,
|
||||
bool emulate_250_ms_sample_drop,
|
||||
bool provide_gps,
|
||||
double final_roll_deg,
|
||||
double gyro_scale)
|
||||
{
|
||||
static fusion_engine_t engine;
|
||||
memset(&engine, 0, sizeof(engine));
|
||||
fusion_engine_config_t config;
|
||||
fusion_engine_default_config(&config, TEST_TIMEBASE_HZ);
|
||||
if(!fusion_engine_init(&engine, &config))
|
||||
{
|
||||
return (scenario_metrics_t){.name = name, .engine_state = FUSION_ENGINE_FAULT};
|
||||
}
|
||||
|
||||
/* Give calibration an absolute origin, just as the product does when GNSS
|
||||
* becomes available before initial tilt/bias calibration completes. */
|
||||
if(provide_gps)
|
||||
{
|
||||
fusion_position_measurement_t origin = make_gps(1u);
|
||||
(void)fusion_engine_push_position(&engine, &origin, NULL, NULL);
|
||||
}
|
||||
|
||||
gps_event_t events[MAX_GPS_EVENTS];
|
||||
unsigned int event_count = provide_gps ? build_gps_events(events) : 0u;
|
||||
unsigned int event_head = 0u;
|
||||
uint64_t last_forwarded_ticks = 0u;
|
||||
bool have_forwarded = false;
|
||||
scenario_metrics_t metrics;
|
||||
memset(&metrics, 0, sizeof(metrics));
|
||||
metrics.name = name;
|
||||
|
||||
const uint64_t sample_count =
|
||||
(uint64_t)TEST_DURATION_S * TEST_IMU_HZ;
|
||||
for(uint64_t sample = 0u ; sample <= sample_count ; sample++)
|
||||
{
|
||||
uint64_t ticks = (sample * TEST_TIMEBASE_HZ + TEST_IMU_HZ / 2u) /
|
||||
TEST_IMU_HZ;
|
||||
fusion_imu_measurement_t imu = make_imu(
|
||||
ticks, config.eskf.gravity_mps2, radians(final_roll_deg), gyro_scale);
|
||||
bool forward = true;
|
||||
if(emulate_250_ms_sample_drop && have_forwarded &&
|
||||
((ticks - last_forwarded_ticks) < 250000u))
|
||||
{
|
||||
forward = false;
|
||||
}
|
||||
if(forward)
|
||||
{
|
||||
if(!fusion_engine_push_imu(&engine, &imu))
|
||||
{
|
||||
break;
|
||||
}
|
||||
last_forwarded_ticks = ticks;
|
||||
have_forwarded = true;
|
||||
metrics.imu_pushed++;
|
||||
}
|
||||
|
||||
while((event_head < event_count) &&
|
||||
(events[event_head].arrival_ticks <= ticks))
|
||||
{
|
||||
(void)fusion_engine_push_position(
|
||||
&engine, &events[event_head].measurement, NULL, NULL);
|
||||
event_head++;
|
||||
}
|
||||
|
||||
if(engine.state == FUSION_ENGINE_RUNNING)
|
||||
{
|
||||
double horizontal = hypot(engine.current.filter.position_m[0],
|
||||
engine.current.filter.position_m[1]);
|
||||
metrics.peak_horizontal_error_m =
|
||||
fmax(metrics.peak_horizontal_error_m, horizontal);
|
||||
metrics.peak_vertical_error_m =
|
||||
fmax(metrics.peak_vertical_error_m,
|
||||
fabs(engine.current.filter.position_m[2]));
|
||||
}
|
||||
}
|
||||
|
||||
metrics.engine_state = engine.state;
|
||||
if(engine.state == FUSION_ENGINE_RUNNING)
|
||||
{
|
||||
metrics.final_horizontal_error_m =
|
||||
hypot(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 = eskf_speed_mps(&engine.current.filter);
|
||||
metrics.final_roll_error_deg =
|
||||
fabs(degrees(quaternion_roll(engine.current.filter.body_to_nav_quaternion)) -
|
||||
final_roll_deg);
|
||||
metrics.motion_state = engine.current.motion.state;
|
||||
}
|
||||
metrics.gps_accepted = engine.diagnostics.position_accepted;
|
||||
metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
|
||||
metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
|
||||
return metrics;
|
||||
}
|
||||
|
||||
static void print_metrics(const scenario_metrics_t* metrics)
|
||||
{
|
||||
printf("%-20s imu=%4llu state=%u motion=%-20s peak-h=%9.3f m peak-v=%9.3f m "
|
||||
"final-h=%9.3f m final-v=%9.3f m speed=%8.3f m/s "
|
||||
"roll=%7.3f deg gps=%llu/%llu zupt=%llu\n",
|
||||
metrics->name,
|
||||
(unsigned long long)metrics->imu_pushed,
|
||||
(unsigned int)metrics->engine_state,
|
||||
fusion_motion_state_name(metrics->motion_state),
|
||||
metrics->peak_horizontal_error_m,
|
||||
metrics->peak_vertical_error_m,
|
||||
metrics->final_horizontal_error_m,
|
||||
metrics->final_vertical_error_m,
|
||||
metrics->final_speed_mps,
|
||||
metrics->final_roll_error_deg,
|
||||
(unsigned long long)metrics->gps_accepted,
|
||||
(unsigned long long)metrics->gps_rejected,
|
||||
(unsigned long long)metrics->zero_updates);
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
scenario_metrics_t full_rate = run_scenario(
|
||||
"52 Hz full IMU", false, true, 0.0, 1.0);
|
||||
scenario_metrics_t imu_only = run_scenario(
|
||||
"52 Hz, no GNSS", false, false, 0.0, 1.0);
|
||||
scenario_metrics_t residual = run_scenario(
|
||||
"1 deg residual tilt", false, true, 10.0, 0.9);
|
||||
scenario_metrics_t sparse = run_scenario(
|
||||
"250 ms sample-drop", true, true, 0.0, 1.0);
|
||||
printf("Asynchronous stationary tilt: 52 Hz IMU, 5 Hz GNSS, 73 ms GNSS delay\n");
|
||||
print_metrics(&full_rate);
|
||||
print_metrics(&imu_only);
|
||||
print_metrics(&residual);
|
||||
print_metrics(&sparse);
|
||||
|
||||
bool full_rate_ok = (full_rate.engine_state == FUSION_ENGINE_RUNNING) &&
|
||||
(full_rate.peak_horizontal_error_m < 1.0) &&
|
||||
(full_rate.peak_vertical_error_m < 1.0) &&
|
||||
(full_rate.final_speed_mps < 0.05);
|
||||
bool imu_only_ok = (imu_only.engine_state == FUSION_ENGINE_RUNNING) &&
|
||||
(imu_only.gps_accepted == 0u) &&
|
||||
(imu_only.peak_horizontal_error_m < 1.0) &&
|
||||
(imu_only.peak_vertical_error_m < 1.0) &&
|
||||
(imu_only.final_speed_mps < 0.05);
|
||||
bool residual_recovered =
|
||||
(residual.engine_state == FUSION_ENGINE_RUNNING) &&
|
||||
(residual.motion_state == FUSION_MOTION_STATIONARY) &&
|
||||
(residual.final_speed_mps < 0.05) &&
|
||||
(residual.final_horizontal_error_m < 1.0) &&
|
||||
(residual.final_roll_error_deg < 0.5);
|
||||
bool sparse_aliasing_detected_and_recovered =
|
||||
(sparse.engine_state == FUSION_ENGINE_RUNNING) &&
|
||||
(sparse.peak_horizontal_error_m > 3.0) &&
|
||||
(sparse.final_horizontal_error_m < 1.0) &&
|
||||
(sparse.final_speed_mps < 0.10) &&
|
||||
(sparse.motion_state == FUSION_MOTION_STATIONARY);
|
||||
if(!full_rate_ok || !imu_only_ok || !residual_recovered ||
|
||||
!sparse_aliasing_detected_and_recovered)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"FAIL: full-rate propagation and GNSS-aided rest recovery must "
|
||||
"remain bounded; sparse sampling must expose a large transient but "
|
||||
"recover after an external at-rest decision.\n");
|
||||
return 1;
|
||||
}
|
||||
printf("PASS: full-rate and residual-tilt cases remained bounded; sparse "
|
||||
"sampling exposed the transient and external at-rest recovered it.\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -3,11 +3,11 @@
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "eskf.h"
|
||||
#include "fusion_engine.h"
|
||||
#include "fusion_bus.h"
|
||||
#include "fusion_gps_adapter.h"
|
||||
#include "fusion_motion.h"
|
||||
#include "fusion/eskf.h"
|
||||
#include "fusion/fusion_engine.h"
|
||||
#include "fusion/fusion_bus.h"
|
||||
#include "fusion/fusion_gps_adapter.h"
|
||||
#include "fusion/fusion_motion.h"
|
||||
|
||||
#define TEST_PI 3.14159265358979323846264338327950288
|
||||
|
||||
@@ -78,6 +78,47 @@ static void test_stationary_propagation(void)
|
||||
CHECK(eskf_state_is_finite(&filter), "stationary state remains finite");
|
||||
}
|
||||
|
||||
static void test_zero_update_primitives(void)
|
||||
{
|
||||
eskf_t filter;
|
||||
eskf_config_t config;
|
||||
identity_filter(&filter, &config);
|
||||
filter.velocity_mps[0] = 1.2;
|
||||
filter.velocity_mps[1] = -0.8;
|
||||
filter.velocity_mps[2] = 0.3;
|
||||
CHECK(eskf_zero_velocity_update(&filter, 0.03),
|
||||
"direct ZUPT measurement update succeeds");
|
||||
CHECK(eskf_speed_mps(&filter) < 0.005,
|
||||
"direct ZUPT drives a stale velocity estimate close to zero");
|
||||
|
||||
double stationary_gyro[3] = {0.0010, -0.0006, 0.0003};
|
||||
double gyro_std[3] = {0.0002, 0.0002, 0.0002};
|
||||
for(unsigned int update = 0u ; update < 20u ; update++)
|
||||
{
|
||||
CHECK(eskf_zero_angular_rate_update(
|
||||
&filter, stationary_gyro, gyro_std),
|
||||
"direct zero-angular-rate update succeeds");
|
||||
}
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
CHECK(fabs(filter.gyro_bias_rps[axis] - stationary_gyro[axis]) < 1.0e-5,
|
||||
"zero-angular-rate update converges gyro bias to stationary reading");
|
||||
}
|
||||
CHECK(eskf_state_is_finite(&filter),
|
||||
"ZUPT and zero-angular-rate updates preserve finite ESKF state");
|
||||
for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
CHECK(filter.covariance[row][row] > 0.0,
|
||||
"zero updates preserve positive covariance diagonal");
|
||||
for(unsigned int col = 0u ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
CHECK(fabs(filter.covariance[row][col] -
|
||||
filter.covariance[col][row]) < 1.0e-12,
|
||||
"zero updates preserve covariance symmetry");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void test_gravity_alignment(void)
|
||||
{
|
||||
eskf_t filter;
|
||||
@@ -446,6 +487,7 @@ static void initialize_engine_for_replay(fusion_engine_t* engine)
|
||||
imu.accel_mps2[2] = config.eskf.gravity_mps2;
|
||||
imu.temperature_c = 25.0;
|
||||
imu.sequence = (uint32_t)(tick / 10u);
|
||||
imu.vehicle_at_rest = true;
|
||||
CHECK(fusion_engine_push_imu(engine, &imu),
|
||||
"stationary calibration IMU accepted");
|
||||
}
|
||||
@@ -475,6 +517,7 @@ static void push_replay_imu(fusion_engine_t* engine, uint64_t tick)
|
||||
imu.accel_mps2[2] = engine->config.eskf.gravity_mps2;
|
||||
imu.temperature_c = 25.0;
|
||||
imu.sequence = (uint32_t)(tick / 10u);
|
||||
imu.vehicle_at_rest = fabs(imu.accel_mps2[0]) < 0.1;
|
||||
CHECK(fusion_engine_push_imu(engine, &imu), "replay profile IMU accepted");
|
||||
}
|
||||
|
||||
@@ -703,6 +746,7 @@ static void test_history_rolls_checkpoint_forward(void)
|
||||
int main(void)
|
||||
{
|
||||
test_stationary_propagation();
|
||||
test_zero_update_primitives();
|
||||
test_gravity_alignment();
|
||||
test_position_nis_gate();
|
||||
test_motion_transitions();
|
||||
|
||||
@@ -0,0 +1,131 @@
|
||||
#include "fusion/fusion_rest_detector.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
static int failures;
|
||||
|
||||
#define CHECK(condition, message) do { \
|
||||
if(!(condition)) { \
|
||||
fprintf(stderr, "FAIL: %s\n", message); \
|
||||
failures++; \
|
||||
} \
|
||||
} while(0)
|
||||
|
||||
static imu_sample_t sample_at(uint64_t ticks, uint64_t timebase_hz,
|
||||
double gyro_x_rps, unsigned int sequence)
|
||||
{
|
||||
imu_sample_t sample;
|
||||
memset(&sample, 0, sizeof(sample));
|
||||
sample.timestamp_ticks = ticks;
|
||||
double time_s = (double)ticks / (double)timebase_hz;
|
||||
sample.accel_mps2[0] = 0.004 * sin(11.0 * time_s);
|
||||
sample.accel_mps2[1] = 0.004 * cos(13.0 * time_s);
|
||||
sample.accel_mps2[2] = 9.80665 + 0.004 * sin(17.0 * time_s);
|
||||
sample.gyro_rps[0] = gyro_x_rps;
|
||||
sample.gyro_rps[1] = 0.0002 * sin(7.0 * time_s);
|
||||
sample.gyro_rps[2] = 0.0002 * cos(9.0 * time_s);
|
||||
sample.temperature_c = 25.0;
|
||||
sample.sequence = sequence;
|
||||
return sample;
|
||||
}
|
||||
|
||||
static void test_rate(unsigned int rate_hz)
|
||||
{
|
||||
const uint64_t timebase_hz = 1000000u;
|
||||
fusion_rest_detector_config_t config;
|
||||
fusion_rest_detector_t detector;
|
||||
fusion_rest_detector_default_config(&config);
|
||||
CHECK(fusion_rest_detector_init(
|
||||
&detector, &config, timebase_hz, true),
|
||||
"rest detector initializes in unarmed/landed mode");
|
||||
|
||||
bool initial_rest_seen = false;
|
||||
bool impulse_cleared_rest = false;
|
||||
double reentry_time_s = NAN;
|
||||
uint64_t sample_count = 6u * rate_hz;
|
||||
for(uint64_t index = 0u ; index <= sample_count ; index++)
|
||||
{
|
||||
uint64_t ticks = (index * timebase_hz + rate_hz / 2u) / rate_hz;
|
||||
double time_s = (double)ticks / (double)timebase_hz;
|
||||
double gyro_x = ((time_s >= 2.0) &&
|
||||
(time_s < 2.0 + 2.0 / (double)rate_hz)) ? 4.0 : 0.0;
|
||||
imu_sample_t sample = sample_at(
|
||||
ticks, timebase_hz, gyro_x, (unsigned int)index);
|
||||
CHECK(fusion_rest_detector_update(&detector, &sample),
|
||||
"monotonic IMU sample updates rest detector");
|
||||
if((time_s >= 1.0) && (time_s < 2.0) && detector.at_rest)
|
||||
{
|
||||
initial_rest_seen = true;
|
||||
}
|
||||
if((time_s >= 2.0) && (time_s < 2.2) && !detector.at_rest)
|
||||
{
|
||||
impulse_cleared_rest = true;
|
||||
}
|
||||
if((time_s > 2.0) && !isfinite(reentry_time_s) && detector.at_rest)
|
||||
{
|
||||
reentry_time_s = time_s;
|
||||
}
|
||||
}
|
||||
CHECK(initial_rest_seen, "quiet landed platform reaches at-rest after dwell");
|
||||
CHECK(impulse_cleared_rest, "high-frequency gyro impulse clears at-rest");
|
||||
CHECK(isfinite(reentry_time_s) && (reentry_time_s >= 3.0) &&
|
||||
(reentry_time_s <= 5.5),
|
||||
"rest returns only after vibration decay and one-second dwell");
|
||||
printf("rest-detector %3u Hz: reentry=%.3f s accel-vibe=%.6f gyro-vibe=%.6f\n",
|
||||
rate_hz, reentry_time_s,
|
||||
detector.accel_vibration_metric_mps2,
|
||||
detector.gyro_vibration_metric_rps);
|
||||
}
|
||||
|
||||
static void test_platform_gate_and_timestamps(void)
|
||||
{
|
||||
const uint64_t timebase_hz = 1000000u;
|
||||
fusion_rest_detector_config_t config;
|
||||
fusion_rest_detector_t detector;
|
||||
fusion_rest_detector_default_config(&config);
|
||||
CHECK(fusion_rest_detector_init(
|
||||
&detector, &config, timebase_hz, false),
|
||||
"rest detector initializes in mobile/not-landed mode");
|
||||
for(unsigned int index = 0u ; index <= 150u ; index++)
|
||||
{
|
||||
imu_sample_t sample = sample_at(
|
||||
(uint64_t)index * 10000u, timebase_hz, 0.0, index);
|
||||
CHECK(fusion_rest_detector_update(&detector, &sample),
|
||||
"mobile quiet samples remain valid");
|
||||
}
|
||||
CHECK(!detector.at_rest,
|
||||
"quiet IMU cannot authorize ZUPT while platform is not landed");
|
||||
|
||||
imu_sample_t duplicate = sample_at(1500000u, timebase_hz, 0.0, 151u);
|
||||
CHECK(!fusion_rest_detector_update(&detector, &duplicate),
|
||||
"duplicate timestamp is rejected");
|
||||
fusion_rest_detector_set_landed(&detector, true, 1500000u);
|
||||
for(unsigned int index = 151u ; index <= 260u ; index++)
|
||||
{
|
||||
imu_sample_t sample = sample_at(
|
||||
(uint64_t)index * 10000u, timebase_hz, 0.0, index);
|
||||
CHECK(fusion_rest_detector_update(&detector, &sample),
|
||||
"landed quiet samples update rest detector");
|
||||
}
|
||||
CHECK(detector.at_rest,
|
||||
"external landed fact plus quiet dwell authorizes at-rest");
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
test_rate(52u);
|
||||
test_rate(104u);
|
||||
test_rate(208u);
|
||||
test_platform_gate_and_timestamps();
|
||||
if(failures != 0)
|
||||
{
|
||||
fprintf(stderr, "%d rest-detector test(s) failed\n", failures);
|
||||
return 1;
|
||||
}
|
||||
printf("PX4-style platform rest detector tests passed.\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -4,7 +4,7 @@
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "fusion_engine.h"
|
||||
#include "fusion/fusion_engine.h"
|
||||
|
||||
#define SYNTH_PI 3.14159265358979323846264338327950288
|
||||
#define SYNTH_TIMEBASE_HZ 1000000u
|
||||
@@ -311,6 +311,9 @@ static synth_metrics_t run_dynamic_scenario(uint32_t scenario_seed)
|
||||
}
|
||||
imu.temperature_c = 25.0 + 0.5 * sin((double)tick / 10000000.0);
|
||||
imu.sequence = (uint32_t)(tick / imu_period_ticks);
|
||||
imu.vehicle_at_rest =
|
||||
(norm3(truth.velocity_mps) < 0.05) &&
|
||||
(norm3(acceleration_nav) < 0.05) && (fabs(yaw_rate) < 0.02);
|
||||
CHECK(fusion_engine_push_imu(&engine, &imu), "dynamic synthetic IMU accepted");
|
||||
if(engine.state == FUSION_ENGINE_FAULT)
|
||||
{
|
||||
@@ -462,8 +465,8 @@ static void check_dynamic_metrics(const synth_metrics_t* metrics)
|
||||
"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->zero_updates > 150u,
|
||||
"stationary periods apply PX4-rate 5 Hz ZUPT updates");
|
||||
CHECK(metrics->horizontal_rmse_m < 2.0,
|
||||
"bias/noise/delay trajectory horizontal RMSE is bounded");
|
||||
CHECK(metrics->vertical_rmse_m < 2.5,
|
||||
@@ -521,6 +524,7 @@ static stationary_metrics_t run_stationary_500hz_scenario(void)
|
||||
}
|
||||
imu.temperature_c = 24.0;
|
||||
imu.sequence = (uint32_t)(tick / imu_period_ticks);
|
||||
imu.vehicle_at_rest = true;
|
||||
CHECK(fusion_engine_push_imu(&engine, &imu), "500 Hz stationary IMU accepted");
|
||||
|
||||
if((tick >= next_gps_tick) && !gps_pending)
|
||||
@@ -575,8 +579,8 @@ static void check_stationary_metrics(const stationary_metrics_t* metrics)
|
||||
"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->zero_updates >= 120u,
|
||||
"30 s / 500 Hz stationary run keeps PX4-rate 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,
|
||||
|
||||
@@ -0,0 +1,178 @@
|
||||
#include "fusion/fusion_engine.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#define TEST_TIMEBASE_HZ 1000000u
|
||||
#define TEST_IMU_HZ 52u
|
||||
#define TEST_GPS_PERIOD_TICKS 200000u
|
||||
|
||||
static fusion_imu_measurement_t stationary_imu(uint64_t timestamp_ticks,
|
||||
double gravity_mps2)
|
||||
{
|
||||
fusion_imu_measurement_t imu;
|
||||
memset(&imu, 0, sizeof(imu));
|
||||
imu.timestamp_ticks = timestamp_ticks;
|
||||
imu.accel_mps2[2] = gravity_mps2;
|
||||
imu.temperature_c = 25.0;
|
||||
imu.vehicle_at_rest = true;
|
||||
return imu;
|
||||
}
|
||||
|
||||
static fusion_position_measurement_t gps_position(
|
||||
uint64_t timestamp_ticks,
|
||||
double east_m,
|
||||
double north_m,
|
||||
double up_m)
|
||||
{
|
||||
fusion_position_measurement_t gps;
|
||||
memset(&gps, 0, sizeof(gps));
|
||||
gps.timestamp_ticks = timestamp_ticks;
|
||||
gps.position_enu_m[0] = east_m;
|
||||
gps.position_enu_m[1] = north_m;
|
||||
gps.position_enu_m[2] = up_m;
|
||||
gps.covariance_enu_m2[0] = 4.0;
|
||||
gps.covariance_enu_m2[4] = 4.0;
|
||||
gps.covariance_enu_m2[8] = 16.0;
|
||||
gps.source_quality = 100.0;
|
||||
gps.source_valid = true;
|
||||
return gps;
|
||||
}
|
||||
|
||||
static bool calibrate_at_origin(fusion_engine_t* engine,
|
||||
fusion_engine_config_t* config)
|
||||
{
|
||||
fusion_engine_default_config(config, TEST_TIMEBASE_HZ);
|
||||
if(!fusion_engine_init(engine, config))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
fusion_position_measurement_t origin = gps_position(1u, 0.0, 0.0, 0.0);
|
||||
if(!fusion_engine_push_position(engine, &origin, NULL, NULL))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for(uint64_t sample = 0u ; sample <= 3u * TEST_IMU_HZ ; sample++)
|
||||
{
|
||||
uint64_t ticks = sample * TEST_TIMEBASE_HZ / TEST_IMU_HZ;
|
||||
fusion_imu_measurement_t imu = stationary_imu(ticks, config->eskf.gravity_mps2);
|
||||
if(!fusion_engine_push_imu(engine, &imu))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return engine->state == FUSION_ENGINE_RUNNING;
|
||||
}
|
||||
|
||||
static int startup_offset_recovers(uint64_t gps_period_ticks)
|
||||
{
|
||||
static fusion_engine_t engine;
|
||||
fusion_engine_config_t config;
|
||||
if(!calibrate_at_origin(&engine, &config))
|
||||
{
|
||||
fprintf(stderr, "FAIL: calibration did not complete\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* Models a poor first LLH origin followed by the stable, correct fixes seen
|
||||
* on UART. ENU remains relative to the first origin, so the correct fixes
|
||||
* legitimately appear far from the initialized ESKF position. */
|
||||
const double expected[3] = {80.0, 40.0, -170.0};
|
||||
uint64_t next_gps_ticks = 3200000u;
|
||||
for(uint64_t sample = 3u * TEST_IMU_HZ + 1u ;
|
||||
sample <= 12u * TEST_IMU_HZ ; sample++)
|
||||
{
|
||||
uint64_t ticks = sample * TEST_TIMEBASE_HZ / TEST_IMU_HZ;
|
||||
fusion_imu_measurement_t imu = stationary_imu(ticks, config.eskf.gravity_mps2);
|
||||
if(!fusion_engine_push_imu(&engine, &imu))
|
||||
{
|
||||
fprintf(stderr, "FAIL: IMU propagation failed\n");
|
||||
return 1;
|
||||
}
|
||||
while(next_gps_ticks <= ticks)
|
||||
{
|
||||
fusion_position_measurement_t gps = gps_position(
|
||||
next_gps_ticks, expected[0], expected[1], expected[2]);
|
||||
(void)fusion_engine_push_position(&engine, &gps, NULL, NULL);
|
||||
next_gps_ticks += gps_period_ticks;
|
||||
}
|
||||
}
|
||||
|
||||
double error = sqrt(
|
||||
pow(engine.current.filter.position_m[0] - expected[0], 2.0) +
|
||||
pow(engine.current.filter.position_m[1] - expected[1], 2.0) +
|
||||
pow(engine.current.filter.position_m[2] - expected[2], 2.0));
|
||||
printf("startup offset GPS=%4llu ms: error=%.3f m accepted=%llu "
|
||||
"rejected=%llu recovery=%llu\n",
|
||||
(unsigned long long)(gps_period_ticks / 1000u), error,
|
||||
(unsigned long long)engine.diagnostics.position_accepted,
|
||||
(unsigned long long)engine.diagnostics.position_nis_rejected,
|
||||
(unsigned long long)engine.diagnostics.position_recovery_count);
|
||||
if((error > 5.0) || (engine.diagnostics.position_recovery_count != 1u))
|
||||
{
|
||||
fprintf(stderr, "FAIL: consistent GPS did not recover the wrong startup position\n");
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int isolated_outlier_does_not_reset(void)
|
||||
{
|
||||
static fusion_engine_t engine;
|
||||
fusion_engine_config_t config;
|
||||
if(!calibrate_at_origin(&engine, &config))
|
||||
{
|
||||
fprintf(stderr, "FAIL: outlier scenario calibration did not complete\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
for(uint64_t gps_ticks = 3200000u ; gps_ticks <= 7000000u ;
|
||||
gps_ticks += TEST_GPS_PERIOD_TICKS)
|
||||
{
|
||||
uint64_t imu_ticks = gps_ticks + 10000u;
|
||||
fusion_imu_measurement_t imu = stationary_imu(
|
||||
imu_ticks, config.eskf.gravity_mps2);
|
||||
if(!fusion_engine_push_imu(&engine, &imu))
|
||||
{
|
||||
fprintf(stderr, "FAIL: outlier scenario IMU propagation failed\n");
|
||||
return 1;
|
||||
}
|
||||
bool outlier = (gps_ticks == 5000000u);
|
||||
fusion_position_measurement_t gps = gps_position(
|
||||
gps_ticks, outlier ? 150.0 : 0.0, outlier ? -120.0 : 0.0,
|
||||
outlier ? 80.0 : 0.0);
|
||||
(void)fusion_engine_push_position(&engine, &gps, NULL, NULL);
|
||||
}
|
||||
|
||||
double distance = sqrt(
|
||||
pow(engine.current.filter.position_m[0], 2.0) +
|
||||
pow(engine.current.filter.position_m[1], 2.0) +
|
||||
pow(engine.current.filter.position_m[2], 2.0));
|
||||
printf("single outlier: error=%.3f m rejected=%llu recovery=%llu\n",
|
||||
distance,
|
||||
(unsigned long long)engine.diagnostics.position_nis_rejected,
|
||||
(unsigned long long)engine.diagnostics.position_recovery_count);
|
||||
if((distance > 1.0) || (engine.diagnostics.position_nis_rejected == 0u) ||
|
||||
(engine.diagnostics.position_recovery_count != 0u))
|
||||
{
|
||||
fprintf(stderr, "FAIL: isolated GPS outlier moved/reset the ESKF\n");
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
int failures = startup_offset_recovers(100000u);
|
||||
failures += startup_offset_recovers(200000u);
|
||||
failures += startup_offset_recovers(1000000u);
|
||||
failures += isolated_outlier_does_not_reset();
|
||||
if(failures == 0)
|
||||
{
|
||||
puts("GPS reacquisition tests passed");
|
||||
}
|
||||
return failures == 0 ? 0 : 1;
|
||||
}
|
||||
@@ -4,8 +4,9 @@
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "fusion_engine.h"
|
||||
#include "imu_sensor.h"
|
||||
#include "fusion/fusion_engine.h"
|
||||
#include "adapters/imu_fusion_profile.h"
|
||||
#include "imu/imu_sensor.h"
|
||||
|
||||
#define QUALITY_PI 3.14159265358979323846264338327950288
|
||||
#define QUALITY_TIMEBASE_HZ 1000000u
|
||||
@@ -158,7 +159,7 @@ static quality_metrics_t run_profile(imu_sensor_backend_t backend,
|
||||
{
|
||||
static fusion_engine_t engine;
|
||||
fusion_imu_quality_profile_t profile;
|
||||
CHECK(imu_sensor_quality_profile_for_backend(backend, &profile),
|
||||
CHECK(imu_fusion_quality_profile_for_backend(backend, &profile),
|
||||
"sensor fusion quality profile exists");
|
||||
fusion_engine_config_t config;
|
||||
fusion_engine_default_config(&config, QUALITY_TIMEBASE_HZ);
|
||||
@@ -255,6 +256,8 @@ static quality_metrics_t run_profile(imu_sensor_backend_t backend,
|
||||
}
|
||||
imu.temperature_c = 25.0 + 0.05 * time_s;
|
||||
imu.sequence = (uint32_t)(tick / imu_period_ticks);
|
||||
imu.vehicle_at_rest = !truth_is_dynamic(
|
||||
&truth, acceleration_nav, yaw_rate);
|
||||
CHECK(fusion_engine_push_imu(&engine, &imu),
|
||||
"quality-profile IMU is accepted");
|
||||
|
||||
@@ -353,8 +356,8 @@ static void check_metrics(const quality_metrics_t* metrics,
|
||||
(metrics->final_stationary_reentry_s <=
|
||||
(noisier_st_profile ? 29.5 : 25.0)),
|
||||
"stationary re-entry waits for motion hysteresis and then recovers");
|
||||
CHECK(metrics->zero_updates > (noisier_st_profile ? 100u : 150u),
|
||||
"stationary intervals apply rate-limited zero updates");
|
||||
CHECK(metrics->zero_updates > 90u,
|
||||
"stationary intervals apply PX4-rate zero updates");
|
||||
CHECK(metrics->accepted_gps >= 15u,
|
||||
"GPS updates remain usable with the sensor profile");
|
||||
CHECK(metrics->horizontal_rmse_m < (noisier_st_profile ? 5.0 : 2.0),
|
||||
@@ -371,10 +374,10 @@ int main(void)
|
||||
{
|
||||
fusion_imu_quality_profile_t tdk_profile;
|
||||
fusion_imu_quality_profile_t st_profile;
|
||||
CHECK(imu_sensor_quality_profile_for_backend(IMU_SENSOR_TDK_ICM42688,
|
||||
CHECK(imu_fusion_quality_profile_for_backend(IMU_SENSOR_TDK_ICM42688,
|
||||
&tdk_profile),
|
||||
"TDK profile is valid");
|
||||
CHECK(imu_sensor_quality_profile_for_backend(IMU_SENSOR_ST_LSM6DSOX,
|
||||
CHECK(imu_fusion_quality_profile_for_backend(IMU_SENSOR_ST_LSM6DSOX,
|
||||
&st_profile),
|
||||
"ST profile is valid");
|
||||
CHECK(tdk_profile.nominal_gyro_noise_density_rps_sqrt_hz[0] <
|
||||
|
||||
@@ -0,0 +1,551 @@
|
||||
#include "fusion/fusion_engine.h"
|
||||
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#define TIMEBASE_HZ 1000000u
|
||||
#define DURATION_S 50.0
|
||||
#define PI 3.14159265358979323846264338327950288
|
||||
|
||||
typedef enum {
|
||||
SCENARIO_STATIONARY = 0,
|
||||
SCENARIO_PEDESTRIAN,
|
||||
SCENARIO_CAR,
|
||||
SCENARIO_AIRCRAFT,
|
||||
SCENARIO_COUNT
|
||||
} scenario_kind_t;
|
||||
|
||||
typedef struct {
|
||||
double position_m[3];
|
||||
double velocity_mps[3];
|
||||
double yaw_rad;
|
||||
} truth_t;
|
||||
|
||||
typedef struct {
|
||||
double forward_accel_mps2;
|
||||
double vertical_accel_mps2;
|
||||
double yaw_rate_rps;
|
||||
} command_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t state;
|
||||
} rng_t;
|
||||
|
||||
typedef struct {
|
||||
double horizontal_rmse_m;
|
||||
double final_horizontal_error_m;
|
||||
double maximum_horizontal_error_m;
|
||||
double maximum_burst_state_step_m;
|
||||
uint64_t gps_accepted;
|
||||
uint64_t gps_rejected;
|
||||
uint64_t position_recoveries;
|
||||
uint64_t zero_updates;
|
||||
uint64_t zero_updates_after_startup;
|
||||
bool finite;
|
||||
bool recovered_after_burst;
|
||||
} metrics_t;
|
||||
|
||||
static const char* scenario_name(scenario_kind_t scenario)
|
||||
{
|
||||
static const char* names[] = {"stationary", "pedestrian", "car", "aircraft"};
|
||||
return names[(unsigned int)scenario];
|
||||
}
|
||||
|
||||
static bool parse_scenario(const char* text, scenario_kind_t* scenario_out)
|
||||
{
|
||||
for(unsigned int index = 0u; index < SCENARIO_COUNT; index++)
|
||||
{
|
||||
if(strcmp(text, scenario_name((scenario_kind_t)index)) == 0)
|
||||
{
|
||||
*scenario_out = (scenario_kind_t)index;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static double square(double value)
|
||||
{
|
||||
return value * value;
|
||||
}
|
||||
|
||||
static double rng_uniform(rng_t* rng)
|
||||
{
|
||||
rng->state = 1664525u * rng->state + 1013904223u;
|
||||
return ((double)(rng->state >> 8u) + 0.5) / 16777216.0;
|
||||
}
|
||||
|
||||
static double rng_normal(rng_t* rng)
|
||||
{
|
||||
double u1 = fmax(rng_uniform(rng), 1.0e-12);
|
||||
double u2 = rng_uniform(rng);
|
||||
return sqrt(-2.0 * log(u1)) * cos(2.0 * PI * u2);
|
||||
}
|
||||
|
||||
static command_t command_at(scenario_kind_t scenario, double time_s)
|
||||
{
|
||||
command_t command = {0.0, 0.0, 0.0};
|
||||
if(scenario == SCENARIO_PEDESTRIAN)
|
||||
{
|
||||
if((time_s >= 4.0) && (time_s < 6.0)) command.forward_accel_mps2 = 0.75;
|
||||
else if((time_s >= 14.0) && (time_s < 16.0)) command.forward_accel_mps2 = -0.75;
|
||||
else if((time_s >= 21.0) && (time_s < 23.0)) command.forward_accel_mps2 = 0.60;
|
||||
else if((time_s >= 31.0) && (time_s < 33.0)) command.forward_accel_mps2 = -0.60;
|
||||
if((time_s >= 24.0) && (time_s < 29.0)) command.yaw_rate_rps = 0.10;
|
||||
}
|
||||
else if(scenario == SCENARIO_CAR)
|
||||
{
|
||||
if((time_s >= 4.0) && (time_s < 9.0)) command.forward_accel_mps2 = 4.0;
|
||||
else if((time_s >= 15.0) && (time_s < 18.0)) command.forward_accel_mps2 = -20.0 / 3.0;
|
||||
else if((time_s >= 25.0) && (time_s < 29.0)) command.forward_accel_mps2 = 5.0;
|
||||
else if((time_s >= 39.0) && (time_s < 42.0)) command.forward_accel_mps2 = -20.0 / 3.0;
|
||||
if((time_s >= 31.0) && (time_s < 36.0)) command.yaw_rate_rps = 0.12;
|
||||
}
|
||||
else if(scenario == SCENARIO_AIRCRAFT)
|
||||
{
|
||||
if((time_s >= 4.0) && (time_s < 14.0)) command.forward_accel_mps2 = 8.0;
|
||||
else if((time_s >= 26.0) && (time_s < 29.0)) command.forward_accel_mps2 = 4.0;
|
||||
else if((time_s >= 42.0) && (time_s < 47.0)) command.forward_accel_mps2 = -6.0;
|
||||
if((time_s >= 6.0) && (time_s < 10.0)) command.vertical_accel_mps2 = 2.0;
|
||||
else if((time_s >= 18.0) && (time_s < 22.0)) command.vertical_accel_mps2 = -2.0;
|
||||
if((time_s >= 20.0) && (time_s < 25.0)) command.yaw_rate_rps = 0.20;
|
||||
else if((time_s >= 34.0) && (time_s < 36.0)) command.yaw_rate_rps = -0.35;
|
||||
}
|
||||
return command;
|
||||
}
|
||||
|
||||
static void attitude_at(scenario_kind_t scenario, double time_s,
|
||||
const truth_t* truth, double euler_rad[3],
|
||||
double body_rate_rps[3])
|
||||
{
|
||||
memset(euler_rad, 0, 3u * sizeof(double));
|
||||
memset(body_rate_rps, 0, 3u * sizeof(double));
|
||||
euler_rad[2] = truth->yaw_rad;
|
||||
command_t command = command_at(scenario, time_s);
|
||||
body_rate_rps[2] = command.yaw_rate_rps;
|
||||
if(scenario != SCENARIO_STATIONARY)
|
||||
{
|
||||
return;
|
||||
}
|
||||
const double roll_max = 20.0 * PI / 180.0;
|
||||
const double pitch_max = 12.0 * PI / 180.0;
|
||||
if((time_s >= 10.0) && (time_s < 11.0))
|
||||
{
|
||||
euler_rad[0] = roll_max * (time_s - 10.0);
|
||||
body_rate_rps[0] = roll_max;
|
||||
}
|
||||
else if((time_s >= 11.0) && (time_s < 13.0)) euler_rad[0] = roll_max;
|
||||
else if((time_s >= 13.0) && (time_s < 14.0))
|
||||
{
|
||||
euler_rad[0] = roll_max * (14.0 - time_s);
|
||||
body_rate_rps[0] = -roll_max;
|
||||
}
|
||||
if((time_s >= 18.0) && (time_s < 19.0))
|
||||
{
|
||||
euler_rad[1] = pitch_max * (time_s - 18.0);
|
||||
body_rate_rps[1] = pitch_max;
|
||||
}
|
||||
else if((time_s >= 19.0) && (time_s < 21.0)) euler_rad[1] = pitch_max;
|
||||
else if((time_s >= 21.0) && (time_s < 22.0))
|
||||
{
|
||||
euler_rad[1] = pitch_max * (22.0 - time_s);
|
||||
body_rate_rps[1] = -pitch_max;
|
||||
}
|
||||
}
|
||||
|
||||
static void rotation_body_to_nav(const double euler[3], double rotation[3][3])
|
||||
{
|
||||
double cr = cos(euler[0]);
|
||||
double sr = sin(euler[0]);
|
||||
double cp = cos(euler[1]);
|
||||
double sp = sin(euler[1]);
|
||||
double cy = cos(euler[2]);
|
||||
double sy = sin(euler[2]);
|
||||
rotation[0][0] = cy * cp;
|
||||
rotation[0][1] = cy * sp * sr - sy * cr;
|
||||
rotation[0][2] = cy * sp * cr + sy * sr;
|
||||
rotation[1][0] = sy * cp;
|
||||
rotation[1][1] = sy * sp * sr + cy * cr;
|
||||
rotation[1][2] = sy * sp * cr - cy * sr;
|
||||
rotation[2][0] = -sp;
|
||||
rotation[2][1] = cp * sr;
|
||||
rotation[2][2] = cp * cr;
|
||||
}
|
||||
|
||||
static void propagate_truth(truth_t* truth, scenario_kind_t scenario,
|
||||
double start_s, double dt_s)
|
||||
{
|
||||
command_t command = command_at(scenario, start_s + 0.5 * dt_s);
|
||||
double speed = hypot(truth->velocity_mps[0], truth->velocity_mps[1]);
|
||||
double yaw_mid = truth->yaw_rad + 0.5 * command.yaw_rate_rps * dt_s;
|
||||
double acceleration[3] = {
|
||||
command.forward_accel_mps2 * cos(yaw_mid) - speed * command.yaw_rate_rps * sin(yaw_mid),
|
||||
command.forward_accel_mps2 * sin(yaw_mid) + speed * command.yaw_rate_rps * cos(yaw_mid),
|
||||
command.vertical_accel_mps2
|
||||
};
|
||||
for(unsigned int axis = 0u; axis < 3u; axis++)
|
||||
{
|
||||
truth->position_m[axis] += truth->velocity_mps[axis] * dt_s +
|
||||
0.5 * acceleration[axis] * dt_s * dt_s;
|
||||
truth->velocity_mps[axis] += acceleration[axis] * dt_s;
|
||||
}
|
||||
truth->yaw_rad += command.yaw_rate_rps * dt_s;
|
||||
}
|
||||
|
||||
static bool externally_at_rest(scenario_kind_t scenario, double time_s)
|
||||
{
|
||||
if(time_s < 3.5) return true;
|
||||
if(scenario != SCENARIO_STATIONARY) return false;
|
||||
/* An external landed/rest supervisor owns this decision. Attitude changes
|
||||
* and the vibration interval revoke the constraint; quiet IMU alone never
|
||||
* invents rest for the moving scenarios. */
|
||||
return !(((time_s >= 9.8) && (time_s < 15.0)) ||
|
||||
((time_s >= 17.8) && (time_s < 23.0)) ||
|
||||
((time_s >= 27.0) && (time_s < 30.0)));
|
||||
}
|
||||
|
||||
static fusion_imu_measurement_t make_imu(scenario_kind_t scenario,
|
||||
const truth_t* truth, double time_s,
|
||||
uint64_t ticks, double gravity,
|
||||
unsigned int sequence, rng_t* rng)
|
||||
{
|
||||
command_t command = command_at(scenario, time_s);
|
||||
double speed = hypot(truth->velocity_mps[0], truth->velocity_mps[1]);
|
||||
double acceleration_nav[3] = {
|
||||
command.forward_accel_mps2 * cos(truth->yaw_rad) -
|
||||
speed * command.yaw_rate_rps * sin(truth->yaw_rad),
|
||||
command.forward_accel_mps2 * sin(truth->yaw_rad) +
|
||||
speed * command.yaw_rate_rps * cos(truth->yaw_rad),
|
||||
command.vertical_accel_mps2 + gravity
|
||||
};
|
||||
double euler[3];
|
||||
double body_rate[3];
|
||||
double rotation[3][3];
|
||||
attitude_at(scenario, time_s, truth, euler, body_rate);
|
||||
rotation_body_to_nav(euler, rotation);
|
||||
fusion_imu_measurement_t imu;
|
||||
memset(&imu, 0, sizeof(imu));
|
||||
imu.timestamp_ticks = ticks;
|
||||
for(unsigned int body = 0u; body < 3u; body++)
|
||||
{
|
||||
for(unsigned int nav = 0u; nav < 3u; nav++)
|
||||
{
|
||||
imu.accel_mps2[body] += rotation[nav][body] * acceleration_nav[nav];
|
||||
}
|
||||
static const double accel_bias[3] = {0.035, -0.025, 0.055};
|
||||
static const double gyro_bias[3] = {0.0008, -0.0006, 0.0007};
|
||||
imu.accel_mps2[body] += accel_bias[body] + 0.012 * rng_normal(rng);
|
||||
imu.gyro_rps[body] = body_rate[body] + gyro_bias[body] +
|
||||
0.00035 * rng_normal(rng);
|
||||
}
|
||||
if((scenario == SCENARIO_STATIONARY) &&
|
||||
(time_s >= 27.0) && (time_s < 29.0))
|
||||
{
|
||||
imu.accel_mps2[0] += 0.80 * sin(2.0 * PI * 7.0 * time_s);
|
||||
imu.accel_mps2[1] += 0.55 * sin(2.0 * PI * 11.0 * time_s);
|
||||
imu.gyro_rps[2] += 0.035 * sin(2.0 * PI * 5.0 * time_s);
|
||||
}
|
||||
imu.temperature_c = 25.0 + 0.5 * sin(0.08 * time_s);
|
||||
imu.sequence = sequence;
|
||||
imu.vehicle_at_rest = externally_at_rest(scenario, time_s);
|
||||
return imu;
|
||||
}
|
||||
|
||||
static fusion_position_measurement_t make_gps(const truth_t* truth,
|
||||
double time_s, uint64_t ticks,
|
||||
rng_t* rng, double gps_position[3])
|
||||
{
|
||||
double horizontal_std = time_s < 8.0 ? 8.0 : 4.0;
|
||||
double vertical_std = time_s < 8.0 ? 12.0 : 7.0;
|
||||
for(unsigned int axis = 0u; axis < 3u; axis++)
|
||||
{
|
||||
double std = axis < 2u ? horizontal_std : vertical_std;
|
||||
gps_position[axis] = truth->position_m[axis] + std * rng_normal(rng);
|
||||
}
|
||||
if((time_s >= 32.0) && (time_s < 34.0))
|
||||
{
|
||||
gps_position[0] += 130.0;
|
||||
gps_position[1] -= 90.0;
|
||||
gps_position[2] += 55.0;
|
||||
}
|
||||
fusion_position_measurement_t gps;
|
||||
memset(&gps, 0, sizeof(gps));
|
||||
gps.timestamp_ticks = ticks;
|
||||
memcpy(gps.position_enu_m, gps_position, sizeof(gps.position_enu_m));
|
||||
gps.covariance_enu_m2[0] = square(horizontal_std);
|
||||
gps.covariance_enu_m2[4] = square(horizontal_std);
|
||||
gps.covariance_enu_m2[8] = square(vertical_std);
|
||||
/* Exactly four satellites have zero residual degrees of freedom. This is
|
||||
* therefore a valid WLS output with covariance but no redundant FDE test. */
|
||||
gps.source_quality = 100.0;
|
||||
gps.source_valid = true;
|
||||
return gps;
|
||||
}
|
||||
|
||||
static void local_to_geodetic(const double position[3], double* latitude_deg,
|
||||
double* longitude_deg, double* height_m)
|
||||
{
|
||||
const double origin_latitude_deg = 40.78925034214207;
|
||||
const double origin_longitude_deg = 29.46277530841728;
|
||||
const double origin_height_m = 100.0;
|
||||
const double latitude_rad = origin_latitude_deg * PI / 180.0;
|
||||
*latitude_deg = origin_latitude_deg + position[1] / 6378137.0 * 180.0 / PI;
|
||||
*longitude_deg = origin_longitude_deg +
|
||||
position[0] / (6378137.0 * cos(latitude_rad)) * 180.0 / PI;
|
||||
*height_m = origin_height_m + position[2];
|
||||
}
|
||||
|
||||
static void write_location(FILE* log_file, const char* source,
|
||||
scenario_kind_t scenario, double time_s,
|
||||
const double position[3], unsigned int gps_ms,
|
||||
unsigned int imu_ms)
|
||||
{
|
||||
if(log_file == NULL) return;
|
||||
double latitude;
|
||||
double longitude;
|
||||
double height;
|
||||
local_to_geodetic(position, &latitude, &longitude, &height);
|
||||
fprintf(log_file,
|
||||
"[LOCATION] source=%s scenario=%s t=%.3f lat=%.12f lon=%.12f "
|
||||
"height=%.3f gdop=3.2 pdop=2.6 hdop=2.0 vdop=1.7 tdop=1.8 "
|
||||
"tracked=4 used=4 gps_ms=%u imu_ms=%u\n",
|
||||
source, scenario_name(scenario), time_s, latitude, longitude,
|
||||
height, gps_ms, imu_ms);
|
||||
}
|
||||
|
||||
static metrics_t run_case(scenario_kind_t scenario, unsigned int gps_ms,
|
||||
unsigned int imu_ms, FILE* log_file)
|
||||
{
|
||||
static fusion_engine_t engine;
|
||||
memset(&engine, 0, sizeof(engine));
|
||||
fusion_engine_config_t config;
|
||||
fusion_engine_default_config(&config, TIMEBASE_HZ);
|
||||
config.calibration_duration_s = 2.0;
|
||||
/* Calibration is time-based for asynchronous sensors; retain enough samples
|
||||
* to estimate variance without silently assuming a particular ODR. */
|
||||
config.calibration_min_samples = 2u * 1000u / imu_ms + 1u;
|
||||
metrics_t metrics;
|
||||
memset(&metrics, 0, sizeof(metrics));
|
||||
if(!fusion_engine_init(&engine, &config)) return metrics;
|
||||
|
||||
truth_t truth;
|
||||
memset(&truth, 0, sizeof(truth));
|
||||
rng_t imu_rng = {0x7a31f20du ^ ((uint32_t)scenario << 24u) ^ imu_ms};
|
||||
rng_t gps_rng = {0xb41d93c7u ^ ((uint32_t)scenario << 20u) ^ gps_ms};
|
||||
uint64_t imu_period_ticks = (uint64_t)imu_ms * 1000u;
|
||||
uint64_t gps_period_ticks = (uint64_t)gps_ms * 1000u;
|
||||
uint64_t next_imu_ticks = 0u;
|
||||
uint64_t next_gps_ticks = 0u;
|
||||
uint64_t next_plot_ticks = 0u;
|
||||
uint64_t end_ticks = (uint64_t)(DURATION_S * TIMEBASE_HZ);
|
||||
uint64_t last_ticks = 0u;
|
||||
unsigned int imu_sequence = 0u;
|
||||
double error_squared_sum = 0.0;
|
||||
uint64_t error_count = 0u;
|
||||
uint64_t accepted_at_burst_end = 0u;
|
||||
uint64_t zero_updates_at_startup_end = 0u;
|
||||
double previous_horizontal[2] = {0.0, 0.0};
|
||||
bool have_previous = false;
|
||||
|
||||
if(log_file != NULL)
|
||||
{
|
||||
fprintf(log_file, "# ESKF synthetic replay; blue=GPS red=IMU-corrected green=truth\n");
|
||||
fprintf(log_file, "# scenario=%s gps_ms=%u imu_ms=%u satellites=4\n",
|
||||
scenario_name(scenario), gps_ms, imu_ms);
|
||||
}
|
||||
while(true)
|
||||
{
|
||||
uint64_t ticks = next_imu_ticks;
|
||||
if(next_gps_ticks < ticks) ticks = next_gps_ticks;
|
||||
if(next_plot_ticks < ticks) ticks = next_plot_ticks;
|
||||
if(ticks > end_ticks) break;
|
||||
double start_s = (double)last_ticks / TIMEBASE_HZ;
|
||||
double dt_s = (double)(ticks - last_ticks) / TIMEBASE_HZ;
|
||||
propagate_truth(&truth, scenario, start_s, dt_s);
|
||||
last_ticks = ticks;
|
||||
double time_s = (double)ticks / TIMEBASE_HZ;
|
||||
|
||||
if(ticks == next_gps_ticks)
|
||||
{
|
||||
/* A 3 s dropout exercises asynchronous dead reckoning. */
|
||||
if(!((time_s >= 25.0) && (time_s < 28.0)))
|
||||
{
|
||||
double gps_position[3];
|
||||
fusion_position_measurement_t gps = make_gps(
|
||||
&truth, time_s, ticks, &gps_rng, gps_position);
|
||||
eskf_update_result_t result = ESKF_UPDATE_INVALID;
|
||||
(void)fusion_engine_push_position(&engine, &gps, &result, NULL);
|
||||
write_location(log_file, "gps", scenario, time_s, gps_position,
|
||||
gps_ms, imu_ms);
|
||||
}
|
||||
next_gps_ticks += gps_period_ticks;
|
||||
}
|
||||
|
||||
if(ticks == next_imu_ticks)
|
||||
{
|
||||
fusion_imu_measurement_t imu = make_imu(
|
||||
scenario, &truth, time_s, ticks, config.eskf.gravity_mps2,
|
||||
imu_sequence++, &imu_rng);
|
||||
if(!fusion_engine_push_imu(&engine, &imu)) break;
|
||||
next_imu_ticks += imu_period_ticks;
|
||||
}
|
||||
|
||||
if(engine.state == FUSION_ENGINE_RUNNING)
|
||||
{
|
||||
if((time_s >= 4.0) && (zero_updates_at_startup_end == 0u))
|
||||
zero_updates_at_startup_end =
|
||||
engine.diagnostics.zero_velocity_update_count;
|
||||
double horizontal_error = hypot(
|
||||
engine.current.filter.position_m[0] - truth.position_m[0],
|
||||
engine.current.filter.position_m[1] - truth.position_m[1]);
|
||||
metrics.maximum_horizontal_error_m = fmax(
|
||||
metrics.maximum_horizontal_error_m, horizontal_error);
|
||||
if(time_s >= 8.0)
|
||||
{
|
||||
error_squared_sum += square(horizontal_error);
|
||||
error_count++;
|
||||
}
|
||||
if(have_previous && (time_s >= 32.0) && (time_s < 34.2))
|
||||
{
|
||||
double step = hypot(
|
||||
engine.current.filter.position_m[0] - previous_horizontal[0],
|
||||
engine.current.filter.position_m[1] - previous_horizontal[1]);
|
||||
metrics.maximum_burst_state_step_m = fmax(
|
||||
metrics.maximum_burst_state_step_m, step);
|
||||
}
|
||||
previous_horizontal[0] = engine.current.filter.position_m[0];
|
||||
previous_horizontal[1] = engine.current.filter.position_m[1];
|
||||
have_previous = true;
|
||||
if((time_s >= 34.0) && (accepted_at_burst_end == 0u))
|
||||
accepted_at_burst_end = engine.diagnostics.position_accepted;
|
||||
}
|
||||
|
||||
if(ticks == next_plot_ticks)
|
||||
{
|
||||
write_location(log_file, "trajectory", scenario, time_s,
|
||||
truth.position_m, gps_ms, imu_ms);
|
||||
if(engine.state == FUSION_ENGINE_RUNNING)
|
||||
write_location(log_file, "imu_corrected", scenario, time_s,
|
||||
engine.current.filter.position_m, gps_ms, imu_ms);
|
||||
next_plot_ticks += 200000u;
|
||||
}
|
||||
}
|
||||
|
||||
double final_error = hypot(
|
||||
engine.current.filter.position_m[0] - truth.position_m[0],
|
||||
engine.current.filter.position_m[1] - truth.position_m[1]);
|
||||
metrics.horizontal_rmse_m = error_count > 0u
|
||||
? sqrt(error_squared_sum / (double)error_count) : INFINITY;
|
||||
metrics.final_horizontal_error_m = final_error;
|
||||
metrics.gps_accepted = engine.diagnostics.position_accepted;
|
||||
metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
|
||||
metrics.position_recoveries = engine.diagnostics.position_recovery_count;
|
||||
metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
|
||||
metrics.zero_updates_after_startup = metrics.zero_updates >= zero_updates_at_startup_end
|
||||
? metrics.zero_updates - zero_updates_at_startup_end : metrics.zero_updates;
|
||||
metrics.recovered_after_burst = engine.diagnostics.position_accepted >
|
||||
accepted_at_burst_end;
|
||||
metrics.finite = (engine.state == FUSION_ENGINE_RUNNING) &&
|
||||
eskf_state_is_finite(&engine.current.filter) && isfinite(final_error);
|
||||
return metrics;
|
||||
}
|
||||
|
||||
static void print_metrics(scenario_kind_t scenario, unsigned int gps_ms,
|
||||
unsigned int imu_ms, const metrics_t* metrics)
|
||||
{
|
||||
printf("%-10s GPS=%4ums IMU=%3ums rmse=%7.2fm final=%7.2fm max=%7.2fm "
|
||||
"burst-step=%6.2fm gps=%3llu/%2llu recovery=%llu zupt=%llu(+%llu) finite=%u\n",
|
||||
scenario_name(scenario), gps_ms, imu_ms, metrics->horizontal_rmse_m,
|
||||
metrics->final_horizontal_error_m, metrics->maximum_horizontal_error_m,
|
||||
metrics->maximum_burst_state_step_m,
|
||||
(unsigned long long)metrics->gps_accepted,
|
||||
(unsigned long long)metrics->gps_rejected,
|
||||
(unsigned long long)metrics->position_recoveries,
|
||||
(unsigned long long)metrics->zero_updates,
|
||||
(unsigned long long)metrics->zero_updates_after_startup,
|
||||
metrics->finite ? 1u : 0u);
|
||||
}
|
||||
|
||||
static int run_matrix(void)
|
||||
{
|
||||
static const unsigned int gps_periods_ms[] = {200u, 500u, 1000u};
|
||||
static const unsigned int imu_periods_ms[] = {19u, 20u, 200u, 300u};
|
||||
bool passed = true;
|
||||
printf("Four-satellite asynchronous platform matrix; 3 s outage and 2 s coherent GNSS fault\n");
|
||||
for(unsigned int scenario = 0u; scenario < SCENARIO_COUNT; scenario++)
|
||||
{
|
||||
for(unsigned int gps_index = 0u; gps_index < 3u; gps_index++)
|
||||
{
|
||||
for(unsigned int imu_index = 0u; imu_index < 4u; imu_index++)
|
||||
{
|
||||
metrics_t metrics = run_case((scenario_kind_t)scenario,
|
||||
gps_periods_ms[gps_index], imu_periods_ms[imu_index], NULL);
|
||||
print_metrics((scenario_kind_t)scenario, gps_periods_ms[gps_index],
|
||||
imu_periods_ms[imu_index], &metrics);
|
||||
bool common_ok = metrics.finite && metrics.recovered_after_burst &&
|
||||
(metrics.position_recoveries == 0u) &&
|
||||
(metrics.maximum_burst_state_step_m < 40.0);
|
||||
bool rest_ok = (scenario == SCENARIO_STATIONARY)
|
||||
? (metrics.zero_updates_after_startup > 0u)
|
||||
: (metrics.zero_updates_after_startup == 0u);
|
||||
bool production_rate_ok = (imu_periods_ms[imu_index] > 20u) ||
|
||||
(metrics.final_horizontal_error_m < 25.0);
|
||||
passed = passed && common_ok && rest_ok && production_rate_ok;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(!passed)
|
||||
{
|
||||
fprintf(stderr, "FAIL: platform scenario matrix contract.\n");
|
||||
return 1;
|
||||
}
|
||||
printf("PASS: 19/20 ms production rates stayed bounded; 200/300 ms coarse IMU "
|
||||
"rates remained finite and their dynamic accuracy loss is reported, not hidden.\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
if(argc == 1) return run_matrix();
|
||||
if((argc == 6) && (strcmp(argv[1], "--log") == 0))
|
||||
{
|
||||
scenario_kind_t scenario;
|
||||
if(!parse_scenario(argv[2], &scenario))
|
||||
{
|
||||
fprintf(stderr, "unknown scenario: %s\n", argv[2]);
|
||||
return 2;
|
||||
}
|
||||
unsigned int gps_ms = (unsigned int)strtoul(argv[3], NULL, 10);
|
||||
unsigned int imu_ms = (unsigned int)strtoul(argv[4], NULL, 10);
|
||||
if(!((gps_ms == 200u) || (gps_ms == 500u) || (gps_ms == 1000u)) ||
|
||||
!((imu_ms == 19u) || (imu_ms == 20u) ||
|
||||
(imu_ms == 200u) || (imu_ms == 300u)))
|
||||
{
|
||||
fprintf(stderr, "supported GPS: 200/500/1000 ms; IMU: 19/20/200/300 ms\n");
|
||||
return 2;
|
||||
}
|
||||
FILE* log_file = fopen(argv[5], "w");
|
||||
if(log_file == NULL)
|
||||
{
|
||||
fprintf(stderr, "cannot create %s: %s\n", argv[5], strerror(errno));
|
||||
return 2;
|
||||
}
|
||||
metrics_t metrics = run_case(scenario, gps_ms, imu_ms, log_file);
|
||||
if(fclose(log_file) != 0)
|
||||
{
|
||||
fprintf(stderr, "cannot close %s\n", argv[5]);
|
||||
return 2;
|
||||
}
|
||||
print_metrics(scenario, gps_ms, imu_ms, &metrics);
|
||||
printf("wrote replay log: %s\n", argv[5]);
|
||||
return metrics.finite ? 0 : 1;
|
||||
}
|
||||
fprintf(stderr, "usage: %s [--log stationary|pedestrian|car|aircraft "
|
||||
"200|500|1000 19|20|200|300 output.log]\n", argv[0]);
|
||||
return 2;
|
||||
}
|
||||
@@ -9,7 +9,7 @@
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "gps_time_anchor.h"
|
||||
#include "gps/gps_time_anchor.h"
|
||||
|
||||
#define MAX_SATELLITES 9
|
||||
#define STATE_DIM 4
|
||||
|
||||
@@ -1,5 +1,12 @@
|
||||
TARGET := gps_system
|
||||
|
||||
# Keep the standalone app build observable on the board. The board and parent
|
||||
# software Makefiles already pass these values, but a direct `make` in this
|
||||
# directory previously compiled every GPS_LOG call out and produced a silent
|
||||
# UART image.
|
||||
GPS_DEBUG ?= 1
|
||||
GPS_DEBUG_LEVEL ?= 4
|
||||
|
||||
# The current Genesys2 Rocket/fabric domain that clocks the UART is 50 MHz.
|
||||
# Keep the requested 115200 baud physical rather than producing 57600 baud
|
||||
# with the common 100 MHz fallback divider.
|
||||
@@ -9,55 +16,59 @@ BSP_UART_CLOCK_HZ ?= 50000000
|
||||
# Accepted values: auto, tdk, st (stm is an alias for st).
|
||||
IMU_SENSOR ?= auto
|
||||
ifeq ($(IMU_SENSOR),auto)
|
||||
IMU_DRIVER_SRCS := src/tdk_icm42688.c src/st_lsm6dsox.c
|
||||
IMU_DRIVER_SRCS := src/drivers/imu/tdk_icm42688.c src/drivers/imu/st_lsm6dsox.c
|
||||
IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_AUTO=1
|
||||
else ifeq ($(IMU_SENSOR),tdk)
|
||||
IMU_DRIVER_SRCS := src/tdk_icm42688.c
|
||||
IMU_DRIVER_SRCS := src/drivers/imu/tdk_icm42688.c
|
||||
IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_TDK=1
|
||||
else ifeq ($(IMU_SENSOR),st)
|
||||
IMU_DRIVER_SRCS := src/st_lsm6dsox.c
|
||||
IMU_DRIVER_SRCS := src/drivers/imu/st_lsm6dsox.c
|
||||
IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_ST=1
|
||||
else ifeq ($(IMU_SENSOR),stm)
|
||||
IMU_DRIVER_SRCS := src/st_lsm6dsox.c
|
||||
IMU_DRIVER_SRCS := src/drivers/imu/st_lsm6dsox.c
|
||||
IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_ST=1
|
||||
else
|
||||
$(error IMU_SENSOR must be auto, tdk, st or stm)
|
||||
endif
|
||||
|
||||
# Zero means use the selected backend's default. The legacy TDK variable is
|
||||
# accepted so existing build commands remain valid.
|
||||
# A non-zero value is a sensor-independent target. The common layer accepts
|
||||
# only a hardware ODR within 5%; thus 200 selects 200 Hz on TDK and the real
|
||||
# 208 Hz setting on LSM6DSOX. Zero keeps the backend default.
|
||||
ifdef TDK_IMU_ODR_HZ
|
||||
IMU_ODR_HZ ?= $(TDK_IMU_ODR_HZ)
|
||||
else
|
||||
IMU_ODR_HZ ?= 0
|
||||
IMU_ODR_HZ ?= 50
|
||||
endif
|
||||
IMU_ACCEL_FS_G ?= 0
|
||||
IMU_GYRO_FS_DPS ?= 0
|
||||
GPS_ESKF_ENABLE ?= 1
|
||||
|
||||
APP_SRCS := \
|
||||
gps_test.c \
|
||||
src/imu_calibration.c \
|
||||
src/imu_sensor.c \
|
||||
src/imu_sensor_profile.c \
|
||||
gps_system.c \
|
||||
src/gps/gps_receiver.c \
|
||||
src/imu/imu_calibration.c \
|
||||
src/imu/imu_sensor.c \
|
||||
src/adapters/imu_fusion_profile.c \
|
||||
src/adapters/gps_system_uart.c \
|
||||
$(IMU_DRIVER_SRCS) \
|
||||
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 \
|
||||
src/gps_pps.c \
|
||||
src/linked_list.c \
|
||||
src/NavEquations.c \
|
||||
src/matrixLib.c
|
||||
src/fusion/eskf.c \
|
||||
src/fusion/fusion_motion.c \
|
||||
src/fusion/fusion_rest_detector.c \
|
||||
src/fusion/fusion_engine.c \
|
||||
src/fusion/fusion_gps_adapter.c \
|
||||
src/fusion/navigation_fusion.c \
|
||||
src/adapters/gps_fusion_publisher.c \
|
||||
src/gps/gps_ip.c \
|
||||
src/gps/gps_interrupt_handlers.c \
|
||||
src/gps/gps_time_anchor.c \
|
||||
src/gps/gps_pps.c \
|
||||
src/gps/linked_list.c \
|
||||
src/gps/NavEquations.c \
|
||||
src/gps/matrixLib.c
|
||||
|
||||
BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c
|
||||
|
||||
GPS_NAV_SOLVE_TIMER_TICKS ?= 50
|
||||
GPS_NAV_SOLVE_TIMER_TICKS ?= 10
|
||||
|
||||
include ../common/app.mk
|
||||
|
||||
@@ -66,6 +77,7 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \
|
||||
-DIMU_ODR_HZ=$(IMU_ODR_HZ) \
|
||||
-DIMU_ACCEL_FS_G=$(IMU_ACCEL_FS_G) \
|
||||
-DIMU_GYRO_FS_DPS=$(IMU_GYRO_FS_DPS) \
|
||||
-DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \
|
||||
-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS)
|
||||
LDLIBS += -lm
|
||||
|
||||
|
||||
@@ -2,25 +2,87 @@
|
||||
|
||||
Bu notlar GPS bare-metal yaziliminin VCU118 Rocket SoC portu ve interrupt debug sureci icin yazildi.
|
||||
|
||||
## Uygulama akisi ve dosya sinirlari
|
||||
|
||||
Firmware giris noktasi [gps_system.c](gps_system.c)'dir. `main()` surucu veya
|
||||
ESKF matematigi icermez; fakat sistemlerin nasil baglandigini acikca gosterir.
|
||||
ESKF enable/disable, GPS WLS sonucu, timestamp'li IMU girdisi, GPS'in fusion
|
||||
olcumune donusumu ve ham/duzeltilmis lat-lon bu dosyada gorunur.
|
||||
Main'in GPS/IMU/fusion arasinda tasidigi mesajlar ve yasam-dongusu bayraklari
|
||||
`header/gps_system.h` icindeki `gps_system_state_t` tipindedir; uygulama veri
|
||||
tipleri giris noktasi C dosyasinda tekrar tanimlanmaz.
|
||||
|
||||
```text
|
||||
gps_system.c
|
||||
-> initialize GPS, IMU and optional ESKF
|
||||
-> wfi
|
||||
-> timestamped IMU queue -> navigation_fusion prediction
|
||||
-> GPS events -> coherent WLS -> gps_nav_solution_t
|
||||
-> valid GPS -> fusion_gps_measurement_t -> navigation_fusion update
|
||||
-> navigation_fusion_output_t -> corrected lat/lon publish
|
||||
```
|
||||
|
||||
Kaynak ve public header agaci ayni modul sinirlarini izler:
|
||||
|
||||
```text
|
||||
header/ + src/
|
||||
gps/ acquisition, tracking, nav epoch, WLS ve GPS interrupt state
|
||||
imu/ sensor secimi, kalibrasyon ve ortak imu_sample_t yonetimi
|
||||
drivers/imu/ yalniz cihaza ozel SPI/register/INT1 kodu
|
||||
fusion/ ESKF, motion/ZUPT, NIS ve delayed-measurement replay
|
||||
adapters/ GPS->fusion ve sensor-profile->fusion donusumleri
|
||||
```
|
||||
|
||||
UART konum/status formatlamasi `adapters/gps_system_uart` icindedir; bu sayede
|
||||
`gps_system.c` telemetry format string'leriyle sismez. Buna karsilik GPS WLS
|
||||
sonucunun fusion olcumune cevrilip ESKF'ye teslim edilmesi bilincli olarak
|
||||
`main()` icinde gorunur kalir.
|
||||
|
||||
GPS kaynaklari IMU veya ESKF header'i include etmez; IMU suruculeri de
|
||||
GPS/fusion header'i include etmez. Ortak trap/PLIC dispatcher harici bir IRQ
|
||||
numarasi ile callback kabul eder. `BSP_PMOD_IRQ -> imu_sensor_irq_handler`
|
||||
baglantisini GPS icinde hard-code etmek yerine `main`, GPS init'inden sonra ve
|
||||
IMU start'indan once kaydeder. GPS ve IMU girdileri yalniz `gps_system.c`
|
||||
uzerinden `navigation_fusion` API'sine verilir; tip donusumleri `adapters/`
|
||||
sinirinda kalir.
|
||||
|
||||
ESKF'nin public girisi `fusion/navigation_fusion.h`, sensor yonetiminin public
|
||||
girisi `imu/imu_sensor.h`, GPS alicisinin public girisi `gps/gps_receiver.h`dir.
|
||||
Yeni sensor eklemek icin yeni bir `drivers/imu` backend'i ve sensor profil
|
||||
adapter'i eklenir; GPS veya ESKF cekirdegi degistirilmez.
|
||||
|
||||
ISR icinde WLS veya ESKF calismaz. IMU ISR'i ornegi olcum aninda timestamp'ler
|
||||
ve raw kuyruga koyar. GPS ISR'leri acquisition/tracking olaylarini kaydeder.
|
||||
`wfi` sonrasi foreground bu kuyruklari tuketir. WLS sirasinda yeni IMU
|
||||
interrupt'lari gelebilecegi icin IMU kuyrugu WLS'den hem once hem sonra drain
|
||||
edilir; GPS update'i WLS bitis zamaninda degil capture epoch'unda uygulanir.
|
||||
|
||||
## Generic IMU Mimarisi (TDK ve ST)
|
||||
|
||||
GPS/ESKF artik dogrudan bir sensor surucusune bagli degildir. Veri yolu:
|
||||
|
||||
```text
|
||||
TDK ICM42688 -----\
|
||||
-> imu_sensor -> imu_calibration -> fusion_imu_measurement -> motion detector + ESKF
|
||||
ST LSM6DSOX ------/
|
||||
-> imu_sensor -> imu_calibration -> imu_sample_t
|
||||
ST LSM6DSOX ------/ |
|
||||
v
|
||||
GPS receiver -> WLS -> gps_nav_solution_t -> GPS adapter -> navigation_fusion
|
||||
|
|
||||
v
|
||||
motion/ZUPT + ESKF -> fused LLH
|
||||
```
|
||||
|
||||
- `src/tdk_icm42688.c`: TDK ICM42688'e ozel SPI, register, interrupt ve
|
||||
- `src/drivers/imu/tdk_icm42688.c`: TDK ICM42688'e ozel SPI, register, interrupt ve
|
||||
ham-veri donusumu.
|
||||
- `src/st_lsm6dsox.c`: STEVAL-MKI217V1 uzerindeki ST LSM6DSOX'e ozel SPI,
|
||||
- `src/drivers/imu/st_lsm6dsox.c`: STEVAL-MKI217V1 uzerindeki ST LSM6DSOX'e ozel SPI,
|
||||
register, INT1 data-ready interrupt ve ham-veri donusumu.
|
||||
- `src/imu_sensor.c`: ortak init/start/stop/IRQ/service/measurement arayuzu.
|
||||
- `src/imu_calibration.c`: markadan bagimsiz bias, 3x3 scale/cross-axis/mounting
|
||||
- `src/imu/imu_sensor.c`: ortak init/start/stop/IRQ/service/measurement arayuzu.
|
||||
- `src/imu/imu_calibration.c`: markadan bagimsiz bias, 3x3 scale/cross-axis/mounting
|
||||
donusumu ve opsiyonel sicaklik katsayisi.
|
||||
- `fusion_measurement.h`, motion detector ve ESKF yalniz SI birimindeki ortak
|
||||
mesaji gorur; aktif sensorun markasini bilmez.
|
||||
- `header/imu/imu_sample.h`: driver ve IMU yonetimi ile fusion arasindaki
|
||||
sensor-markasindan bagimsiz SI mesaj sozlesmesi.
|
||||
- `src/adapters/imu_fusion_profile.c`: aktif sensorun karakteristigini ESKF
|
||||
process-noise/kalibrasyon esiklerine cevirir.
|
||||
|
||||
Firmware isletim sistemi kullanmaz. Yeni IMU katmani `malloc` cagirmadan sensor
|
||||
kuyruklarini ve butun durumunu statik ayirir. Newlib ve formatli UART destegi
|
||||
@@ -47,6 +109,17 @@ edilerek 512 KiB sinirinda dogrulanmistir.
|
||||
|
||||
### Bare-metal sensor secimi
|
||||
|
||||
ESKF/fusion varsayilan olarak aciktir. GPS-only firmware icin ana uygulama
|
||||
akisini degistirmeden kapatilabilir:
|
||||
|
||||
```sh
|
||||
make -C soc/software/gps_system GPS_ESKF_ENABLE=0
|
||||
```
|
||||
|
||||
Bu durumda GPS acquisition/tracking/WLS ve ham lat-lon yayini calismaya devam
|
||||
eder; IMU stream'i ve `navigation_fusion` baslatilmaz. UART startup satirinda
|
||||
`eskf_enabled` ve gercek init sonucu olan `eskf_ready` ayri alanlardir.
|
||||
|
||||
Otomatik WHO_AM_I algilama varsayilandir:
|
||||
|
||||
```sh
|
||||
@@ -61,7 +134,8 @@ make -C soc/software/gps_system IMU_SENSOR=tdk \
|
||||
IMU_ODR_HZ=500 IMU_ACCEL_FS_G=4 IMU_GYRO_FS_DPS=250
|
||||
|
||||
make -C soc/software/gps_system IMU_SENSOR=st \
|
||||
IMU_ODR_HZ=104 IMU_ACCEL_FS_G=2 IMU_GYRO_FS_DPS=250
|
||||
IMU_ODR_HZ=50 GPS_NAV_SOLVE_TIMER_TICKS=10 \
|
||||
IMU_ACCEL_FS_G=2 IMU_GYRO_FS_DPS=250
|
||||
```
|
||||
|
||||
`IMU_SENSOR=stm`, `st` seciminin alias'idir. `IMU_ODR_HZ=0`,
|
||||
@@ -70,6 +144,33 @@ secer. Bu ozellikle `auto` icin onemlidir; TDK ve ST'nin ortak bir ODR degeri
|
||||
yoktur. Desteklenen ODR'ler TDK icin 25/50/100/200/500/1000 Hz, ST icin
|
||||
52/104/208 Hz'dir.
|
||||
|
||||
`IMU_ODR_HZ` fiziksel data-ready hizini secer; ST icin `50` istegi gercek 52 Hz
|
||||
ayarina eslenir. Her IMU ornegi kendi monotonik CLINT timestamp'i ile ESKF
|
||||
tahminine girer. 19/20 ms gibi sabit bir `dt` varsayilmaz ve CPU yuku icin ham
|
||||
ornek atlanmaz. Ileride ESKF orani dusurulecekse PX4'teki gibi delta-angle ve
|
||||
delta-velocity biriktiren, coning/sculling bilgisini koruyan bir downsampler
|
||||
kullanilmalidir.
|
||||
|
||||
GPS orani bundan bagimsizdir. `GPS_NAV_SOLVE_TIMER_TICKS=5/10/50` sirasiyla
|
||||
100/200/1000 ms nav-solve periyodu verir. WLS sonucu gec gelse bile olcum kendi
|
||||
capture timestamp'inde fixed-lag replay ile uygulanir; yalniz
|
||||
`solution_valid=true` olan GPS cozumu fusion'a girer. GPS yokken IMU ESKF'nin
|
||||
process-model tahminini surdurur, fakat filtre kendine sahte konum olcumu
|
||||
vermez. Global origin yoksa LLH cikisi valid degildir; origin daha once
|
||||
kurulduysa sonuc yardimsiz dead-reckoning'dir ve kovaryansi zamanla buyur.
|
||||
|
||||
NIS kapisi tek bir aykiri GPS'in ESKF'yi suruklemesini engeller. Ancak acilista
|
||||
ilk GPS fix'i kotu olup sonraki tutarli fix'ler bu ilk noktadan onlarca metre
|
||||
uzakta kalirsa, yalniz hard NIS rejection kullanmak filtreyi yanlis baslangica
|
||||
kilitler. Fusion engine bu nedenle ardisik reddedilen position fix'lerinin
|
||||
zamansal surekliligini izler. Son başarılı fusion'dan sonra yedi saniye timeout
|
||||
dolmuşsa ve en az uc fix birbirinin devamı
|
||||
ise yalniz ESKF konumu ve konum kovaryansi son olcume yeniden ankrajlanir;
|
||||
attitude, hiz ve IMU bias state'leri korunur, gecersiz hale gelen position
|
||||
cross-covariance'lari sifirlanir. Tek aykiri fix bu yolu tetiklemez. Bu davranis
|
||||
100/200/1000 ms GPS oranlarinda `gps_reacquisition_test.c` ile dogrulanir ve
|
||||
level-2 fusion status satirinda `recoveries=` sayaci ile gozlenir.
|
||||
|
||||
TDK ve ST veri-hazir interrupt'i ile calisir ve ana dongu `wfi` kullanabilir.
|
||||
Genesys2 son pin planinda ortak aktif sensor interrupt girişi `JB1/V29`
|
||||
uzerinden wrapper'daki 3FF synchronizer'a, oradan PLIC device 11'e gider. ST
|
||||
@@ -79,12 +180,14 @@ sicaklik alanlarini okuyup statik SPSC kuyruğa koyar. `imu_sensor_service()`
|
||||
interrupt backend'lerde no-op'tur.
|
||||
|
||||
Standalone LSM6DSOX SPI okuma, stationary log ve `INT1 -> JB1 -> 3FF -> PLIC
|
||||
11 -> C ISR` fiziksel yolu kartta dogrulanmistir. 2026-07-16 canli sayac
|
||||
snapshot'inda 4084 interrupt'in 4084'u sample'a donusmus; read-fail, queue
|
||||
overflow, unexpected IRQ ve trap sayilari sifir kalmistir. Buna karsilik GPS +
|
||||
ST + ESKF'nin ayni urun firmware kosusundaki end-to-end testi henuz tamamlanmis
|
||||
sayilmamalidir. Yuksek ODR ve uzun WLS sirasinda sample kaybini daha da azaltmak
|
||||
gerekirse sonraki gelistirme sensor FIFO + sensor timestamp kullanmaktir;
|
||||
11 -> C ISR` fiziksel yolu kartta dogrulanmistir. 2026-07-16 GPS + ST + ESKF
|
||||
end-to-end stationary kosusunda 6531 interrupt/sample, 467 ESKF aktarimi ve
|
||||
322 kabul edilen GPS update'i goruldu; read-fail, queue overflow, missed sample
|
||||
ve GPS NIS reject sayilari sifir kaldi. Ayni logun yerlesmis son bolumunde 100
|
||||
ham GPS fix'inin yatay RMS sacilimi 13.62 m, son 20 fused durumun yatay RMS
|
||||
sacilimi 0.39 m oldu. Bu degerler survey referansina gore mutlak dogruluk degil,
|
||||
sabit anten etrafindaki repeatability olcumudur. Yuksek ODR gerekirse sonraki
|
||||
gelistirme sensor FIFO + sensor timestamp veya daha yuksek CPU butcesidir;
|
||||
ortak IMU arayuzu ve ESKF bunun icin degismeyecektir.
|
||||
|
||||
Sensor ODR'si ile GPS nav-solve periyodu birbirinden bagimsizdir. Her IMU ve GPS
|
||||
@@ -93,6 +196,30 @@ ESKF olaylari timestamp sirasinda isler. TDK ve ST ISR timestamp'i data-ready
|
||||
kenarini yakindan temsil eder. Daha kesin ve WLS/interrupt latency'sinden
|
||||
bagimsiz yuksek-ODR zamanlama gerekirse sensor FIFO timestamp'i kullanilmalidir.
|
||||
|
||||
### Level-1 konum UART protokolu
|
||||
|
||||
Her gecerli GPS navigation epoch'unda level 1'de iki konum satiri art arda
|
||||
yayinlanir. Ilk satir WLS GPS konumu, ikinci satir o anda mevcut IMU+ESKF
|
||||
duzeltilmis konumdur:
|
||||
|
||||
```text
|
||||
[LOCATION] source=gps lat=... lon=... height=... gdop=... pdop=... hdop=... vdop=... tdop=... tracked=... used=...
|
||||
[LOCATION] source=imu_corrected lat=... lon=... height=... gdop=... pdop=... hdop=... vdop=... tdop=... tracked=... used=...
|
||||
```
|
||||
|
||||
`tracked`, donanimda calisan tracking kanal sayisini; `used`, ilgili WLS
|
||||
cozumunde kullanilan uydu sayisini belirtir. DOP alanlari ayni GPS epoch'unun
|
||||
geometrisidir ve iki satirda ayni tasinir. ESKF henuz kalibrasyon/origin
|
||||
asamasindaysa yalniz GPS satiri yayinlanir; ESKF valid olduktan sonra her GPS
|
||||
konumu geldiginde iki satir da yayinlanir. Eski `[NAV] position` ve periyodik
|
||||
`[FUSION] lat/lon` ayrintilari daha yuksek debug seviyesindedir; level 1
|
||||
parser'inin ayni konumu birden fazla kez gormesi engellenir.
|
||||
|
||||
Desktop `location_plotter`, `source=gps` serisini mavi ve
|
||||
`source=imu_corrected` serisini kirmizi cizer. Iki kaynak arasinda zigzag yol
|
||||
cizmez; corrected veri varsa CEP/RMS analizi yalniz corrected seriyle yapilir.
|
||||
Linux sentetik replay loglarindaki `source=trajectory` gerçek izi yeşil çizer.
|
||||
|
||||
### Kalibrasyon ve sicaklik politikasi
|
||||
|
||||
Uc farkli duzeltme birbirinden ayridir:
|
||||
@@ -142,7 +269,7 @@ Generated DTS/RTL'e gore GPS IP adresleri:
|
||||
| TRCK7 | `0x10028000` | `9` |
|
||||
| TRCK8 | `0x10029000` | `10` |
|
||||
|
||||
Bu degerler `soc/software/bsp/include/bsp/soc.h` icinde BSP define olarak tutuluyor. GPS yazilimi `header/gps_ip_defines.h` icinde bu BSP define'larini kullaniyor.
|
||||
Bu degerler `soc/software/bsp/include/bsp/soc.h` icinde BSP define olarak tutuluyor. GPS yazilimi `header/gps/gps_ip_defines.h` icinde bu BSP define'larini kullaniyor.
|
||||
|
||||
PLIC base:
|
||||
|
||||
@@ -357,14 +484,14 @@ Bu bolum continuous/background acquisition ve parametrik acquisition engine dest
|
||||
|
||||
### Degisen Dosyalar
|
||||
|
||||
- `header/gps_ip_defines.h`
|
||||
- `header/gps_macros.h`
|
||||
- `header/gps_interrupt_handlers.h`
|
||||
- `header/linked_list.h`
|
||||
- `src/gps_ip.c`
|
||||
- `src/gps_interrupt_handlers.c`
|
||||
- `src/linked_list.c`
|
||||
- `gps_test.c`
|
||||
- `header/gps/gps_ip_defines.h`
|
||||
- `header/gps/gps_macros.h`
|
||||
- `header/gps/gps_interrupt_handlers.h`
|
||||
- `header/gps/linked_list.h`
|
||||
- `src/gps/gps_ip.c`
|
||||
- `src/gps/gps_interrupt_handlers.c`
|
||||
- `src/gps/linked_list.c`
|
||||
- `gps_system.c` (onceki `gps_test.c` giris noktasi)
|
||||
|
||||
### Parametric Acquisition Engine Destegi
|
||||
|
||||
@@ -526,8 +653,9 @@ GPS_TRK_MEASUREMENT_STALE_TICKS
|
||||
- Subframe 1 `toc` alani parity bitleri disarida birakilarak 16 bit okunur ve `2^4` ile olceklenir.
|
||||
- Subframe 4 page 18 henuz alinmadiysa iyonosfer duzeltmesi uygulanmaz. Page 18 hazir oldugunda her navigation solve tek bir `ionoutc` kopyasi kullanir.
|
||||
- Klobuchar gecikmesi WLS'nin receiver ECEF, azimut ve elevasyon hesabindan uretilir. Ilk unseeded iteration iyonosfersizdir; yaklasik receiver konumu olustuktan sonraki iteration'larda gecikme gozlenen pseudorange'dan cikarilir.
|
||||
- Bundan sonra periyodik pozisyon hesaplamasi nav-solve interrupt'lari ile tetiklenir. RTL `subframe_ready` sonrasinda COUNT durumuna girer ve `NAV_SOLVE_TIMER=N` icin N adet navigation `bit_valid` darbesi sayar. Her navigation biti 20 ms oldugu icin periyot `N * 20 ms`'dir; mevcut `gps_startup_initialization(10)` ayari 200 ms'de bir ABS_SAMPLE latch ve nav-solve interrupt'i uretir.
|
||||
- Bu siralama onemlidir: RTL `nav_solve_timer` sayaci `NAV_SOLVE_EN` ve `subframe_ready` ile baslar. `NAV_SOLVE_EN` nav-bit asamasinda acik tutulur ki timer GPS bit/subframe fazina otursun; subframe 1..5 hazir olana kadar yalniz nav-solve interrupt/flag kapali kalir.
|
||||
- Bundan sonra periyodik pozisyon hesaplamasi nav-solve interrupt'lari ile tetiklenir. RTL `subframe_ready` sonrasinda COUNT durumuna girer ve `NAV_SOLVE_TIMER=N` icin N adet navigation `bit_valid` darbesi sayar. Her navigation biti 20 ms oldugu icin periyot `N * 20 ms`'dir; firmware `GPS_NAV_SOLVE_TIMER_TICKS` build ayarini `gps_startup_initialization()` fonksiyonuna verir.
|
||||
- Bu siralama onemlidir: RTL `nav_solve_timer` sayaci `NAV_SOLVE_EN` ve ilk gecerli `subframe_ready` ile baslar. `NAV_SOLVE_EN` nav-bit asamasinda acik tutulur; subframe 1..5 hazir olana kadar yalniz nav-solve interrupt/flag kapali kalir. Timer COUNT durumuna girdikten sonra sonraki subframe darbeleri sayaci sifirlamaz veya yeniden fazlamaz.
|
||||
- Tracking kanallari farkli zamanlarda lock olabilir ve ilk gecerli subframe'lerini 6 saniyenin farkli katlarinda gorebilir. Timer bu nedenle ilk subframe'in HOW alanindaki 17-bit Z-count degerini kullanarak `Z_COUNT * 300 mod NAV_SOLVE_TIMER` baslangic fazini hesaplar. Hesap ALIGN durumunda 26 PCLK cevriminde ard arda modulo ile yapilir; 20 ms'lik ilk navigation bitinden cok once tamamlanir. RTL ayni GPS haftasi icinde keyfi pozitif `N` degerlerini hizalayabilir; firmware ise hafta rollover'i oncesinden aktif kalan kanallarla rollover sonrasinda katilan kanallarin da ayni fazda kalmasi icin `30240000 mod N == 0` kosulunu compile time'da zorunlu tutar. Ornegin `N=40`, `N=100`, `N=125`, `N=150`, `N=200` ve `N=300` gecerlidir; kosulu saglamayan build acik bir `#error` ile durur.
|
||||
- Initial position sonrasinda subframe'lerini tamamlayan yeni tracking kanallarinda da nav bits acik tutularak nav-solve otomatik etkinlestirilir.
|
||||
- Coherence kontrolu sadece software systick ile yapilmaz; periodic snapshot icin tracking node'da saklanan nav-solve interrupt sonrasi raw `ABS_SAMPLE` kullanilir. `GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS=25` oldugu icin 4 satellite measurement'i 25 ms'den fazla ayriksa navigation solve calistirilmaz. Bu pencere interrupt skew'den cok uydu path delay farkini kapsamak icindir: 20 ms yaklasik 6000 km, 25 ms yaklasik 7500 km pseudorange farkina denk gelir. 25 ms, dusuk-elevation uydu geometrisi ve tracking jitter icin pay birakir ama 50 ms kadar gevsek degildir.
|
||||
- Partial coherent grup sayisi mevcut `ready channels` sayisina ulasmadiysa firmware nav-solve sequence'lerini consume etmez. Coherent snapshot tum ready kanallari kapsasa bile sequence ancak navigation solve basarili olduktan sonra, snapshot aninda kaydedilen sequence degerine kadar consume edilir. Basarisiz solve ayni epoch'u tekrar deneyebilir; daha yeni sequence yanlislikla tuketilmez.
|
||||
@@ -538,6 +666,7 @@ GPS_TRK_MEASUREMENT_STALE_TICKS
|
||||
- Tracking nav-solve interrupt'lari ayni GPS epoch'unda birbirine cok yakin gelebilir. External interrupt handler bu yuzden PLIC'te bekleyen farkli GPS source ID'lerini tek ISR girisinde bounded olarak drain eder. Ayni source ID tekrar claim edilirse handler durur; bu durum level interrupt clear'in henuz donanima yansimadigini gosterir ve `PLIC repeated source while draining` WARN sayaci ile raporlanir.
|
||||
- Normal nav-bit interrupt'larinda mevcut tracking node tekrar insert edilmeye calisilmaz. Bu nedenle `requested node already exists` UART spam'i normal akista gorulmemelidir; per-subframe `nav bits` logu `TRACE` seviyesine alinmistir.
|
||||
- Nav solve mode'da `NAV_SOLVE_TIMER`, subframe fazindan sonra kac adet 20 ms navigation biti sayilacagini; dolayisiyla ABS_SAMPLE latch ve interrupt epoch'unu belirler. Her islenen event'te TOW `NAV_SOLVE_TIMER * 20 ms` artirilir ve GPS hafta sinirinda normalize edilir. Raw `ABS_SAMPLE` TOW artisini belirlemez; yalniz ayni nav-solve epoch'undan gelen kanallari secmek ve pseudorange olcumunu olusturmak icin kullanilir. Pseudorange hesabina giden `gps_ephems.abs_sample` receiver sample'in corrected double halidir.
|
||||
- `hardware/external/gps/rtl/gps_timer/tb/nav_solve_timer_tb.sv`, RTL'nin ayni hafta icindeki genel fazlama davranisini bir kanalin digerinden tam bir subframe gec basladigi `N=40`, `N=37`, `N=512` ve `N=1` durumlariyla kontrol eder. Firmware build kurali bunlardan GPS haftasindaki `30240000` navigation biti tam bolmeyen degerleri sistem konfigurasyonu olarak reddeder.
|
||||
- Tracking wrapper'daki `REM_CODE_PHASE_REG` donanimda IEEE-754 double degildir. RTL'de code NCO phase accumulator 46 bittir; software'e expose edilen kisim `rem_code_ph[45:20]` yani 26-bit integer residue'dur ve APB `PRDATA[25:0]` alanina zero-extend edilerek verilir. Bu yuzden firmware bu register'i `uint32_t` okur; `uint64_t` veya `double*` olarak MMIO okumak iki 32-bit APB read uretir ve register map ile uyumlu degildir.
|
||||
- Navigation icin kullanilan corrected receiver sample formulu:
|
||||
|
||||
@@ -578,6 +707,11 @@ weight = 1 / variance
|
||||
- Ephemeris alanlarinin mevcut isimleri ile ICD anlamlari su sekildedir: `omgdot` gercekte `Omega0`, `omg0` gercekte argument of perigee `omega`, `omgkdot` gercekte `OmegaDot` degeridir. `satpos()` bunlari dogru matematiksel yerde kullanir; konu su an hesap hatasi degil, ileride yapilacak kontrollu isim duzeltmesidir.
|
||||
- `POSTPONE_TOW=0.160` donanim/pipeline zaman kalibrasyonunun bir parcasidir; kaynagi dogrulanmadan kaldirilmamalidir.
|
||||
- Tum ready kanallarin coherent nav-solve interrupt'inin beklenmesi bilincli davranistir. Donanimin her aktif tracking kanali icin ilgili interrupt'i uretecegi kabul edilir.
|
||||
- Dort uydu 3D konum + receiver clock bias icin minimum ve yeterlidir. Bu durumda
|
||||
`dof=N-4=0` oldugu icin WLS konumu publish edilir fakat residual chi-square
|
||||
integrity bayragi false kalir. Bes veya daha fazla uyduda publish icin mevcut
|
||||
chi-square gecis kosulu uygulanir. Boylece `solution_valid` ile
|
||||
`integrity_available/pass` ayni kavram olarak kullanilmaz.
|
||||
- Broadcast SV health filtresi uygulanir. Sonraki integrity adimi ephemeris age/fit interval ve post-fit residual metriklerini eklemektir. En az bes satellite olmadan tek hatali olcumu residual ile guvenilir sekilde ayirmak mumkun degildir.
|
||||
|
||||
### Debug Mesajlari
|
||||
@@ -688,20 +822,20 @@ Eger ACQ engine'ler tracking bolgesinden once arka arkaya yerlestiriliyorsa `BSP
|
||||
#define BSP_GPS_TRCK0_IRQ 3UL
|
||||
```
|
||||
|
||||
ACQ tarafinda normal durumda `src/gps_ip.c` icinde manuel pointer eklemek gerekmez. Startup'ta `gps_init_acq_tables()` su helper'lari kullanarak tabloyu doldurur:
|
||||
ACQ tarafinda normal durumda `src/gps/gps_ip.c` icinde manuel pointer eklemek gerekmez. Startup'ta `gps_init_acq_tables()` su helper'lari kullanarak tabloyu doldurur:
|
||||
|
||||
```c
|
||||
GPS_ACQ_IP_BASE_ADDR(index)
|
||||
GPS_ACQ_INTR_LINE(index)
|
||||
```
|
||||
|
||||
ACQ engine sayisi firmware tarafinda tek noktadan `header/gps_ip_defines.h` icindeki `ACQ_ENGINE_COUNT` ile kontrol edilir. Varsayilan olarak `BSP_GPS_ACQ_COUNT` kullanilir. Donanim/BSP dogru count veriyorsa ekstra override gerekmez.
|
||||
ACQ engine sayisi firmware tarafinda tek noktadan `header/gps/gps_ip_defines.h` icindeki `ACQ_ENGINE_COUNT` ile kontrol edilir. Varsayilan olarak `BSP_GPS_ACQ_COUNT` kullanilir. Donanim/BSP dogru count veriyorsa ekstra override gerekmez.
|
||||
|
||||
Eger ACQ address veya IRQ offset'leri uniform degilse, mevcut `base + index * stride` modeli yeterli degildir. Bu durumda:
|
||||
|
||||
- `soc.h` icinde her ACQ icin explicit base/IRQ define'i eklenmeli, ornek `BSP_GPS_ACQ0_BASE`, `BSP_GPS_ACQ1_BASE`.
|
||||
- `header/gps_ip_defines.h` icindeki `GPS_ACQ_IP_BASE_ADDR(index)` ve `GPS_ACQ_INTR_LINE(index)` helper'lari explicit tablo veya switch ile guncellenmeli.
|
||||
- `src/gps_ip.c` icindeki `gps_init_acq_tables()` explicit base/IRQ tablolarini kullanacak sekilde guncellenmeli.
|
||||
- `header/gps/gps_ip_defines.h` icindeki `GPS_ACQ_IP_BASE_ADDR(index)` ve `GPS_ACQ_INTR_LINE(index)` helper'lari explicit tablo veya switch ile guncellenmeli.
|
||||
- `src/gps/gps_ip.c` icindeki `gps_init_acq_tables()` explicit base/IRQ tablolarini kullanacak sekilde guncellenmeli.
|
||||
|
||||
ACQ register layout'u degisirse sadece count/base degistirmek yetmez. Su alanlar da XLSX/RTL'e gore kontrol edilmelidir:
|
||||
|
||||
@@ -726,14 +860,14 @@ Uniform tracking channel address/IRQ stride varsa `soc.h` icinde su define'lar g
|
||||
|
||||
Tracking tarafinda mevcut firmware bazi yerlerde hala manuel channel entry'leri kullaniyor. `BSP_GPS_TRCK_COUNT` arttirilinca su dosyalarda yeni kanal icin entry eklenmelidir:
|
||||
|
||||
- `header/gps_ip_defines.h`
|
||||
- `header/gps/gps_ip_defines.h`
|
||||
- `TRCK9_IP_BASE_ADDR`, `TRCK10_IP_BASE_ADDR`, ...
|
||||
- `TRCK9_INTR_LINE`, `TRCK10_INTR_LINE`, ...
|
||||
- `src/gps_ip.c`
|
||||
- `src/gps/gps_ip.c`
|
||||
- `trck9_ip`, `trck10_ip`, ...
|
||||
- `trck_ips[]` array'ine yeni pointer'lar,
|
||||
- `trck_ips_intr_lines[]` array'ine yeni IRQ line'lar.
|
||||
- `src/gps_interrupt_handlers.c`
|
||||
- `src/gps/gps_interrupt_handlers.c`
|
||||
- dosya sonundaki `DEFINE_TRCK_INTR_HANDLER(9)`, `DEFINE_TRCK_INTR_HANDLER(10)`, ... wrapper'lari.
|
||||
|
||||
`GPS_TRCK_IP_COUNT`, `BSP_GPS_TRCK_COUNT` uzerinden gelir ve loop/array boyutlari bu count'u kullanir. Ancak yukaridaki manuel array initializer'lar ve handler wrapper'lari yeni count ile uyumlu olmak zorundadir.
|
||||
@@ -741,8 +875,8 @@ Tracking tarafinda mevcut firmware bazi yerlerde hala manuel channel entry'leri
|
||||
Eger TRCK address veya IRQ offset'leri uniform degilse, mevcut `BSP_GPS_TRCK0_BASE + index * BSP_GPS_TRCK_STRIDE` ve `BSP_GPS_TRCK0_IRQ + index * BSP_GPS_TRCK_IRQ_STRIDE` modeli yeterli degildir. Bu durumda:
|
||||
|
||||
- BSP'ye her tracking channel icin explicit base/IRQ define'i eklenmeli.
|
||||
- `header/gps_ip_defines.h` icindeki `TRCKn_IP_BASE_ADDR` ve `TRCKn_INTR_LINE` define'lari explicit degerlerden gelmeli.
|
||||
- `src/gps_ip.c` icindeki `trck_ips[]` ve `trck_ips_intr_lines[]` array'leri bu explicit define'lari kullanmali.
|
||||
- `header/gps/gps_ip_defines.h` icindeki `TRCKn_IP_BASE_ADDR` ve `TRCKn_INTR_LINE` define'lari explicit degerlerden gelmeli.
|
||||
- `src/gps/gps_ip.c` icindeki `trck_ips[]` ve `trck_ips_intr_lines[]` array'leri bu explicit define'lari kullanmali.
|
||||
|
||||
TRCK register layout'u degisirse su alanlar XLSX/RTL'e gore tekrar kontrol edilmelidir:
|
||||
|
||||
|
||||
@@ -0,0 +1,135 @@
|
||||
#include <bsp/soc.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "adapters/gps_fusion_publisher.h"
|
||||
#include "adapters/gps_system_uart.h"
|
||||
#include "adapters/imu_fusion_profile.h"
|
||||
#include "fusion/navigation_fusion.h"
|
||||
#include "gps/gps_receiver.h"
|
||||
#include "gps_system.h"
|
||||
#include "imu/imu_sensor.h"
|
||||
|
||||
#ifndef GPS_ESKF_ENABLE
|
||||
#define GPS_ESKF_ENABLE 1
|
||||
#endif
|
||||
|
||||
#if (GPS_ESKF_ENABLE != 0) && (GPS_ESKF_ENABLE != 1)
|
||||
#error "GPS_ESKF_ENABLE must be 0 or 1"
|
||||
#endif
|
||||
|
||||
static void initialize_sensor_fusion(gps_system_state_t* system)
|
||||
{
|
||||
system->fusion_enabled = (GPS_ESKF_ENABLE != 0);
|
||||
if(!system->fusion_enabled)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
imu_sensor_config_t imu_config;
|
||||
imu_sensor_default_config(&imu_config);
|
||||
imu_config.odr_hz = IMU_ODR_HZ;
|
||||
imu_config.accel_full_scale_g = IMU_ACCEL_FS_G;
|
||||
imu_config.gyro_full_scale_dps = IMU_GYRO_FS_DPS;
|
||||
system->imu_initialized = imu_sensor_init(&imu_config);
|
||||
|
||||
fusion_imu_quality_profile_t imu_quality;
|
||||
bool quality_available = system->imu_initialized &&
|
||||
imu_fusion_quality_profile_for_backend(
|
||||
imu_sensor_active_backend(), &imu_quality);
|
||||
system->fusion_initialized = quality_available &&
|
||||
navigation_fusion_init(
|
||||
BSP_CLINT_TIMEBASE_HZ, &imu_quality,
|
||||
true); /* Current prototype is the PX4-equivalent unarmed bench mode. */
|
||||
}
|
||||
|
||||
static unsigned int feed_pending_imu_to_eskf(gps_system_state_t* system)
|
||||
{
|
||||
if(!system->fusion_initialized)
|
||||
{
|
||||
return 0u;
|
||||
}
|
||||
|
||||
unsigned int processed = 0u;
|
||||
imu_sensor_service();
|
||||
while(imu_sensor_take_measurement(&system->imu_sample))
|
||||
{
|
||||
(void)navigation_fusion_push_imu(&system->imu_sample);
|
||||
processed++;
|
||||
}
|
||||
return processed;
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
gps_system_state_t system = {0};
|
||||
|
||||
/* GPS and IMU are initialized independently. The application owns their
|
||||
* interrupt connection and decides whether the ESKF path is enabled. */
|
||||
initialize_sensor_fusion(&system);
|
||||
gps_receiver_initialize(GPS_NAV_SOLVE_TIMER_TICKS);
|
||||
bool imu_irq_attached = system.fusion_initialized &&
|
||||
gps_receiver_attach_external_irq(BSP_PMOD_IRQ,
|
||||
imu_sensor_irq_handler);
|
||||
system.imu_running = imu_irq_attached && system.fusion_initialized &&
|
||||
imu_sensor_start();
|
||||
gps_receiver_start_acquisition();
|
||||
gps_system_uart_publish_startup(
|
||||
system.fusion_enabled, system.fusion_initialized,
|
||||
system.imu_initialized, system.imu_running);
|
||||
|
||||
for(;;)
|
||||
{
|
||||
/* GPS, IMU data-ready and the maintenance timer wake this loop. */
|
||||
asm volatile("wfi");
|
||||
|
||||
/* IMU prediction runs at the sensor's own timestamped rate. */
|
||||
(void)feed_pending_imu_to_eskf(&system);
|
||||
|
||||
/* GPS remains an independent receiver: events first, then coherent WLS. */
|
||||
gps_receiver_process_pending_events();
|
||||
system.gps_epoch_ready = gps_receiver_solve_ready_epoch(
|
||||
&system.gps_solution, &system.gps_measurement_ticks);
|
||||
|
||||
/* WLS may take long enough for more IMU samples to arrive. */
|
||||
(void)feed_pending_imu_to_eskf(&system);
|
||||
|
||||
if(system.gps_epoch_ready && system.gps_solution.solution_valid)
|
||||
{
|
||||
/* Raw WLS latitude/longitude remains visible even with ESKF disabled. */
|
||||
gps_system_uart_publish_location("gps",
|
||||
system.gps_solution.latitude_deg,
|
||||
system.gps_solution.longitude_deg,
|
||||
system.gps_solution.height_m,
|
||||
&system.gps_solution);
|
||||
|
||||
/* This is the only GPS -> sensor-fusion hand-off in the application. */
|
||||
bool gps_measurement_ready = system.fusion_initialized &&
|
||||
gps_fusion_build_measurement(
|
||||
&system.gps_solution,
|
||||
system.gps_measurement_ticks,
|
||||
&system.gps_for_fusion);
|
||||
if(gps_measurement_ready)
|
||||
{
|
||||
/* Delayed GPS update is applied at its capture timestamp; newer
|
||||
* IMU history is replayed to produce the current fused state. */
|
||||
(void)navigation_fusion_push_gps(&system.gps_for_fusion);
|
||||
if(navigation_fusion_get_output(&system.fused_solution) &&
|
||||
system.fused_solution.global_valid)
|
||||
{
|
||||
gps_system_uart_publish_location("imu_corrected",
|
||||
system.fused_solution.latitude_deg,
|
||||
system.fused_solution.longitude_deg,
|
||||
system.fused_solution.height_m,
|
||||
&system.gps_solution);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(system.fusion_initialized)
|
||||
{
|
||||
gps_system_uart_publish_periodic_fusion_status(
|
||||
&system.fused_solution, &system.last_status_tick);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,72 +0,0 @@
|
||||
#include <stdio.h>
|
||||
#include "gps_ip_defines.h"
|
||||
#include "linked_list.h"
|
||||
#include <stddef.h>
|
||||
#include <float.h>
|
||||
#include "matrixLib.h"
|
||||
#include "gps_macros.h"
|
||||
#include "imu_sensor.h"
|
||||
#include "navigation_fusion.h"
|
||||
|
||||
int main()
|
||||
{
|
||||
imu_sensor_config_t imu_config;
|
||||
imu_sensor_default_config(&imu_config);
|
||||
imu_config.odr_hz = IMU_ODR_HZ;
|
||||
imu_config.accel_full_scale_g = IMU_ACCEL_FS_G;
|
||||
imu_config.gyro_full_scale_dps = IMU_GYRO_FS_DPS;
|
||||
bool imu_initialized = imu_sensor_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 && imu_sensor_start();
|
||||
(void)imu_running;
|
||||
imu_sensor_diagnostics_t imu_diagnostics;
|
||||
imu_sensor_get_diagnostics(&imu_diagnostics);
|
||||
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
|
||||
"init imu=%u sensor=%s who=0x%02x running=%u odr_hz=%lu "
|
||||
"poll=%u timebase_hz=%lu\r\n",
|
||||
imu_initialized ? 1u : 0u,
|
||||
imu_sensor_backend_name(imu_diagnostics.backend),
|
||||
(unsigned int)imu_diagnostics.who_am_i,
|
||||
imu_running ? 1u : 0u,
|
||||
(unsigned long)imu_diagnostics.configured_odr_hz,
|
||||
imu_diagnostics.foreground_polling ? 1u : 0u,
|
||||
(unsigned long)BSP_CLINT_TIMEBASE_HZ);
|
||||
|
||||
acq_start_loop();
|
||||
uint64_t last_fusion_status_tick = 0u;
|
||||
|
||||
while(1)
|
||||
{
|
||||
/* GPS and either IMU backend wake the shared machine interrupt path. */
|
||||
asm volatile("wfi");
|
||||
gps_background_task();
|
||||
calculate_position();
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
#ifndef GPS_FUSION_PUBLISHER_H
|
||||
#define GPS_FUSION_PUBLISHER_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "gps/NavEquations.h"
|
||||
#include "fusion/fusion_measurement.h"
|
||||
|
||||
/* Thin boundary adapter used by main. GPS and the ESKF do not call each
|
||||
* other; main converts the completed WLS output to the shared measurement. */
|
||||
bool gps_fusion_build_measurement(
|
||||
const gps_nav_solution_t* solution,
|
||||
uint64_t measurement_timestamp_ticks,
|
||||
fusion_gps_measurement_t* measurement_out);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,27 @@
|
||||
#ifndef GPS_SYSTEM_UART_H
|
||||
#define GPS_SYSTEM_UART_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "fusion/navigation_fusion.h"
|
||||
#include "gps/NavEquations.h"
|
||||
|
||||
void gps_system_uart_publish_location(
|
||||
const char* source,
|
||||
double latitude_deg,
|
||||
double longitude_deg,
|
||||
double height_m,
|
||||
const gps_nav_solution_t* gps);
|
||||
|
||||
void gps_system_uart_publish_startup(
|
||||
bool fusion_enabled,
|
||||
bool fusion_initialized,
|
||||
bool imu_initialized,
|
||||
bool imu_running);
|
||||
|
||||
void gps_system_uart_publish_periodic_fusion_status(
|
||||
navigation_fusion_output_t* output,
|
||||
uint64_t* last_status_tick);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,14 @@
|
||||
#ifndef IMU_FUSION_PROFILE_H
|
||||
#define IMU_FUSION_PROFILE_H
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "fusion/fusion_imu_quality.h"
|
||||
#include "imu/imu_sensor.h"
|
||||
|
||||
/* Explicit adapter from a selected sensor backend to ESKF tuning values. */
|
||||
bool imu_fusion_quality_profile_for_backend(
|
||||
imu_sensor_backend_t backend,
|
||||
fusion_imu_quality_profile_t* profile_out);
|
||||
|
||||
#endif
|
||||
+2
-2
@@ -4,7 +4,7 @@
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "fusion_measurement.h"
|
||||
#include "imu/imu_sample.h"
|
||||
|
||||
#ifndef ST_IMU_RAW_QUEUE_CAPACITY
|
||||
#define ST_IMU_RAW_QUEUE_CAPACITY 256u
|
||||
@@ -46,7 +46,7 @@ void st_lsm6dsox_irq_handler(void);
|
||||
/* Retained as a no-op so every backend has the same service interface. */
|
||||
void st_lsm6dsox_service(void);
|
||||
bool st_lsm6dsox_take_measurement(
|
||||
fusion_imu_measurement_t* measurement_out);
|
||||
imu_sample_t* measurement_out);
|
||||
void st_lsm6dsox_get_diagnostics(
|
||||
st_lsm6dsox_diagnostics_t* diagnostics_out);
|
||||
|
||||
+2
-2
@@ -4,7 +4,7 @@
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "fusion_measurement.h"
|
||||
#include "imu/imu_sample.h"
|
||||
|
||||
#ifndef TDK_IMU_RAW_QUEUE_CAPACITY
|
||||
#define TDK_IMU_RAW_QUEUE_CAPACITY 256u
|
||||
@@ -69,7 +69,7 @@ void tdk_icm42688_stop(void);
|
||||
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);
|
||||
bool tdk_icm42688_take_measurement(imu_sample_t* measurement_out);
|
||||
void tdk_icm42688_get_diagnostics(tdk_icm42688_diagnostics_t* diagnostics_out);
|
||||
|
||||
#endif
|
||||
@@ -65,6 +65,18 @@ bool eskf_propagate(eskf_t* filter,
|
||||
const double gyro_rps[ESKF_VECTOR_DIM],
|
||||
double dt_s);
|
||||
|
||||
/* Midpoint inertial integration over two timestamped raw samples. The
|
||||
* constant-input eskf_propagate() entry point remains available for callers
|
||||
* that already provide an interval-integrated/held measurement. */
|
||||
bool eskf_propagate_interval(
|
||||
eskf_t* filter,
|
||||
const eskf_config_t* config,
|
||||
const double start_accel_mps2[ESKF_VECTOR_DIM],
|
||||
const double start_gyro_rps[ESKF_VECTOR_DIM],
|
||||
const double end_accel_mps2[ESKF_VECTOR_DIM],
|
||||
const double end_gyro_rps[ESKF_VECTOR_DIM],
|
||||
double dt_s);
|
||||
|
||||
eskf_update_result_t eskf_position_update(
|
||||
eskf_t* filter,
|
||||
const eskf_config_t* config,
|
||||
@@ -72,6 +84,15 @@ eskf_update_result_t eskf_position_update(
|
||||
const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM],
|
||||
double* nis_out);
|
||||
|
||||
/* Re-anchor position after a higher-level fusion timeout has established that
|
||||
* repeated, valid position observations are mutually coherent. Velocity,
|
||||
* attitude and IMU biases are preserved; position cross-covariances are
|
||||
* cleared because they are no longer valid after the state discontinuity. */
|
||||
bool eskf_reset_position(
|
||||
eskf_t* filter,
|
||||
const double position_m[ESKF_VECTOR_DIM],
|
||||
const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM]);
|
||||
|
||||
bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps);
|
||||
|
||||
bool eskf_zero_angular_rate_update(
|
||||
+1
-1
@@ -4,7 +4,7 @@
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "fusion_measurement.h"
|
||||
#include "fusion/fusion_measurement.h"
|
||||
|
||||
#ifndef FUSION_IMU_QUEUE_CAPACITY
|
||||
#define FUSION_IMU_QUEUE_CAPACITY 256u
|
||||
+19
-4
@@ -4,10 +4,10 @@
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "eskf.h"
|
||||
#include "fusion_imu_quality.h"
|
||||
#include "fusion_measurement.h"
|
||||
#include "fusion_motion.h"
|
||||
#include "fusion/eskf.h"
|
||||
#include "fusion/fusion_imu_quality.h"
|
||||
#include "fusion/fusion_measurement.h"
|
||||
#include "fusion/fusion_motion.h"
|
||||
|
||||
#ifndef FUSION_IMU_HISTORY_CAPACITY
|
||||
#define FUSION_IMU_HISTORY_CAPACITY 2048u
|
||||
@@ -38,8 +38,13 @@ typedef struct {
|
||||
double zero_gyro_std_rps[3];
|
||||
double gnss_motion_speed_threshold_mps;
|
||||
double gnss_displacement_nis_gate;
|
||||
double gnss_motion_window_s;
|
||||
unsigned int gnss_moving_confirmations;
|
||||
unsigned int gnss_still_confirmations;
|
||||
double position_recovery_timeout_s;
|
||||
double position_recovery_max_speed_mps;
|
||||
double position_recovery_sigma_multiplier;
|
||||
unsigned int position_recovery_min_samples;
|
||||
} fusion_engine_config_t;
|
||||
|
||||
typedef struct {
|
||||
@@ -49,13 +54,17 @@ typedef struct {
|
||||
fusion_gnss_motion_evidence_t gnss_evidence;
|
||||
double last_accepted_position_m[3];
|
||||
double last_accepted_position_covariance_m2[9];
|
||||
double zero_gyro_integral_rad[3];
|
||||
double zero_gyro_duration_s;
|
||||
uint64_t filter_timestamp_ticks;
|
||||
uint64_t last_zero_update_ticks;
|
||||
uint64_t last_accepted_position_ticks;
|
||||
uint64_t last_position_fuse_ticks;
|
||||
unsigned int gnss_moving_count;
|
||||
unsigned int gnss_still_count;
|
||||
bool have_held_imu;
|
||||
bool have_accepted_position;
|
||||
bool vehicle_at_rest;
|
||||
} fusion_runtime_t;
|
||||
|
||||
typedef struct {
|
||||
@@ -70,9 +79,11 @@ typedef struct {
|
||||
uint64_t position_stale_rejected;
|
||||
uint64_t position_pending_overflow;
|
||||
uint64_t position_accepted;
|
||||
uint64_t position_recovery_count;
|
||||
uint64_t replay_count;
|
||||
uint64_t replayed_imu_count;
|
||||
uint64_t zero_velocity_update_count;
|
||||
uint64_t zero_gyro_update_count;
|
||||
} fusion_engine_diagnostics_t;
|
||||
|
||||
typedef struct {
|
||||
@@ -95,12 +106,16 @@ typedef struct {
|
||||
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_position_measurement_t position_recovery_candidate;
|
||||
fusion_engine_diagnostics_t diagnostics;
|
||||
uint64_t position_recovery_start_ticks;
|
||||
unsigned int imu_history_head;
|
||||
unsigned int imu_history_count;
|
||||
unsigned int pending_position_count;
|
||||
unsigned int position_recovery_consistent_count;
|
||||
bool checkpoint_valid;
|
||||
bool have_initial_position;
|
||||
bool position_recovery_active;
|
||||
} fusion_engine_t;
|
||||
|
||||
void fusion_engine_default_config(fusion_engine_config_t* config,
|
||||
+1
-1
@@ -3,7 +3,7 @@
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "fusion_measurement.h"
|
||||
#include "fusion/fusion_measurement.h"
|
||||
|
||||
typedef struct {
|
||||
double minimum_horizontal_std_m;
|
||||
+5
@@ -4,6 +4,8 @@
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "imu/imu_sample.h"
|
||||
|
||||
/*
|
||||
* Hardware- and solver-neutral messages shared at the GPS/ESKF boundary.
|
||||
* timestamp_ticks always belongs to the common local monotonic timebase; GPS
|
||||
@@ -23,12 +25,15 @@ typedef struct {
|
||||
bool solution_valid;
|
||||
} fusion_gps_measurement_t;
|
||||
|
||||
/* Timestamped IMU plus the independently supervised platform state consumed
|
||||
* by fusion. Drivers publish imu_sample_t and do not decide `vehicle_at_rest`. */
|
||||
typedef struct {
|
||||
uint64_t timestamp_ticks;
|
||||
double accel_mps2[3];
|
||||
double gyro_rps[3];
|
||||
double temperature_c;
|
||||
uint32_t sequence;
|
||||
bool vehicle_at_rest;
|
||||
} fusion_imu_measurement_t;
|
||||
|
||||
/* Generic local-navigation measurement consumed by the ESKF coordinator. */
|
||||
@@ -0,0 +1,50 @@
|
||||
#ifndef FUSION_REST_DETECTOR_H
|
||||
#define FUSION_REST_DETECTOR_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "imu/imu_sample.h"
|
||||
|
||||
/*
|
||||
* PX4-style platform rest supervisor. The ESKF consumes the resulting
|
||||
* at-rest flag; it does not infer that flag from its own velocity state.
|
||||
* `landed` is an external platform fact (unarmed bench prototype, wheel/leg
|
||||
* contact, land detector, and so on), not a GNSS/ESKF estimate.
|
||||
*/
|
||||
typedef struct {
|
||||
double vibration_filter_time_constant_s;
|
||||
double accel_vibration_limit_mps2;
|
||||
double gyro_vibration_limit_rps;
|
||||
double rest_dwell_s;
|
||||
} fusion_rest_detector_config_t;
|
||||
|
||||
typedef struct {
|
||||
fusion_rest_detector_config_t config;
|
||||
double previous_accel_mps2[3];
|
||||
double previous_gyro_rps[3];
|
||||
double accel_vibration_metric_mps2;
|
||||
double gyro_vibration_metric_rps;
|
||||
uint64_t timebase_hz;
|
||||
uint64_t last_sample_ticks;
|
||||
uint64_t last_motion_ticks;
|
||||
bool have_previous_sample;
|
||||
bool landed;
|
||||
bool at_rest;
|
||||
} fusion_rest_detector_t;
|
||||
|
||||
void fusion_rest_detector_default_config(fusion_rest_detector_config_t* config);
|
||||
bool fusion_rest_detector_init(
|
||||
fusion_rest_detector_t* detector,
|
||||
const fusion_rest_detector_config_t* config,
|
||||
uint64_t timebase_hz,
|
||||
bool landed);
|
||||
void fusion_rest_detector_set_landed(
|
||||
fusion_rest_detector_t* detector,
|
||||
bool landed,
|
||||
uint64_t timestamp_ticks);
|
||||
bool fusion_rest_detector_update(
|
||||
fusion_rest_detector_t* detector,
|
||||
const imu_sample_t* sample);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,64 @@
|
||||
#ifndef NAVIGATION_FUSION_H
|
||||
#define NAVIGATION_FUSION_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "fusion/fusion_engine.h"
|
||||
#include "fusion/fusion_measurement.h"
|
||||
#include "fusion/fusion_motion.h"
|
||||
#include "imu/imu_sample.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;
|
||||
uint64_t gps_position_recoveries;
|
||||
uint64_t zero_update_count;
|
||||
uint64_t replay_count;
|
||||
uint64_t stale_gps_count;
|
||||
uint64_t imu_processed_count;
|
||||
unsigned int calibration_sample_count;
|
||||
fusion_engine_state_t engine_state;
|
||||
bool local_valid;
|
||||
bool global_valid;
|
||||
bool vehicle_at_rest;
|
||||
/* Compatibility alias for UART publishers: valid means global LLH valid. */
|
||||
bool valid;
|
||||
} navigation_fusion_output_t;
|
||||
|
||||
/* Initialize the sensor-independent 15-state ESKF and GPS-to-ENU adapter. */
|
||||
bool navigation_fusion_init(
|
||||
uint64_t local_timebase_hz,
|
||||
const fusion_imu_quality_profile_t* imu_quality,
|
||||
bool platform_landed);
|
||||
|
||||
/* PX4-style external platform fact. For this bench prototype `true` means
|
||||
* unarmed/on-ground; a mobile integration must drive it from its own
|
||||
* arm/contact/land supervisor. */
|
||||
void navigation_fusion_set_platform_landed(
|
||||
bool platform_landed,
|
||||
uint64_t timestamp_ticks);
|
||||
|
||||
/* Prediction input: every timestamped IMU sample is integrated; it is not an
|
||||
* ESKF observation and is never rate-selected in this layer. */
|
||||
bool navigation_fusion_push_imu(const imu_sample_t* measurement);
|
||||
|
||||
/* Observation input: convert a valid LLH fix to local ENU, apply it at the
|
||||
* captured GPS epoch, then replay newer IMU history to current time. */
|
||||
bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement);
|
||||
|
||||
/* local_valid can exist without GPS. global_valid/valid require a GPS origin. */
|
||||
bool navigation_fusion_get_output(navigation_fusion_output_t* output);
|
||||
const char* navigation_fusion_state_name(fusion_engine_state_t state);
|
||||
|
||||
#endif
|
||||
+4
-3
@@ -2,10 +2,10 @@
|
||||
#define NAVEQUATIONS_H
|
||||
|
||||
#include "gps.h"
|
||||
#include "gps_ip_defines.h"
|
||||
#include "matrixLib.h"
|
||||
#include "gps/gps_ip_defines.h"
|
||||
#include "gps/matrixLib.h"
|
||||
|
||||
typedef struct {
|
||||
typedef struct gps_nav_solution {
|
||||
double ecef_m[3];
|
||||
double clock_bias_m;
|
||||
double latitude_deg;
|
||||
@@ -39,6 +39,7 @@
|
||||
double quality_score;
|
||||
unsigned int available_satellite_count;
|
||||
unsigned int used_satellite_count;
|
||||
unsigned int tracked_satellite_count;
|
||||
unsigned int degrees_of_freedom;
|
||||
uint32_t used_row_mask;
|
||||
uint32_t excluded_row_mask;
|
||||
-1
@@ -558,4 +558,3 @@
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
#define GPS_H
|
||||
|
||||
//#include "plutogpssim.h"
|
||||
#include "gps_ip_defines.h"
|
||||
#include "gps/gps_ip_defines.h"
|
||||
|
||||
//#define GM_EARTH 3.986005e14
|
||||
//#define OMEGA_EARTH 7.2921151467e-5
|
||||
+6
-1
@@ -1,7 +1,7 @@
|
||||
#ifndef GPS_INT_HAND
|
||||
#define GPS_INT_HAND
|
||||
|
||||
#include "gps_ip_defines.h"
|
||||
#include "gps/gps_ip_defines.h"
|
||||
|
||||
typedef struct {
|
||||
uint32_t event_overflow_total;
|
||||
@@ -16,9 +16,14 @@
|
||||
uint64_t last_lost_tick;
|
||||
} gps_runtime_diagnostics_t;
|
||||
|
||||
typedef void (*gps_external_irq_handler_t)(void);
|
||||
|
||||
bool assign_tracking_from_result(const gps_acq_result_t* result);
|
||||
void acq_intr_handler(unsigned char engine_id);
|
||||
void gps_dispatch_irq(unsigned int irq_id);
|
||||
bool gps_interrupt_attach_external_source(
|
||||
unsigned int irq_id,
|
||||
gps_external_irq_handler_t handler);
|
||||
void gps_irq_handler(void) GPS_IRQ_HANDLER_ATTR;
|
||||
void gps_process_isr_events(void);
|
||||
void gps_acq_scheduler_run(void);
|
||||
+14
-5
@@ -6,8 +6,8 @@
|
||||
#include <float.h>
|
||||
#include <stdint.h>
|
||||
#include <bsp/soc.h>
|
||||
#include "matrixLib.h"
|
||||
#include "gps_macros.h"
|
||||
#include "gps/matrixLib.h"
|
||||
#include "gps/gps_macros.h"
|
||||
|
||||
#define GPS_IRQ_HANDLER_ATTR __attribute__((interrupt("machine"), aligned(4)))
|
||||
|
||||
@@ -49,6 +49,7 @@
|
||||
#ifndef GPS_NAV_SOLVE_TIMER_TICKS
|
||||
#define GPS_NAV_SOLVE_TIMER_TICKS 50u
|
||||
#endif
|
||||
#define GPS_NAV_BITS_PER_WEEK 30240000u
|
||||
#define GPS_NAV_SOLVE_PERIOD_MS (GPS_NAV_SOLVE_TIMER_TICKS * GPS_NAV_SOLVE_TIMER_TICK_MS)
|
||||
/* Keep integrity statistics active, but do not let leave-one-out FDE
|
||||
* replace the full-WLS position until the real measurement-noise model
|
||||
@@ -64,7 +65,11 @@
|
||||
#define GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC 0.001
|
||||
#define GPS_HOW_ANCHOR_MAX_AGE_SEC 12.0
|
||||
|
||||
_Static_assert(GPS_NAV_SOLVE_TIMER_TICKS > 0u, "NAV solve timer must be non-zero");
|
||||
#if (GPS_NAV_SOLVE_TIMER_TICKS == 0u)
|
||||
#error "GPS_NAV_SOLVE_TIMER_TICKS must be non-zero"
|
||||
#elif ((GPS_NAV_BITS_PER_WEEK % GPS_NAV_SOLVE_TIMER_TICKS) != 0u)
|
||||
#error "GPS_NAV_SOLVE_TIMER_TICKS must divide 30240000 GPS navigation bits per week"
|
||||
#endif
|
||||
_Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u),
|
||||
"GPS_NAV_ENABLE_FDE must be zero or one");
|
||||
|
||||
@@ -460,10 +465,14 @@
|
||||
// parse functions. Generic parse function.
|
||||
unsigned char parse_subframe(int unsigned* subframe, gps_frame_t* gps_frame,
|
||||
unsigned int* how_tow_sec_out);
|
||||
struct gps_nav_solution;
|
||||
#ifdef EKF_ENABLE
|
||||
void calculate_position(Ekf* g_ekf);
|
||||
bool calculate_position(Ekf* g_ekf,
|
||||
struct gps_nav_solution* solution_out,
|
||||
uint64_t* measurement_timestamp_ticks_out);
|
||||
#else
|
||||
void calculate_position();
|
||||
bool calculate_position(struct gps_nav_solution* solution_out,
|
||||
uint64_t* measurement_timestamp_ticks_out);
|
||||
#endif
|
||||
void gps_startup_initialization(unsigned int nsp_tick);
|
||||
void gps_interrupt_init(void);
|
||||
+5
-3
@@ -5,8 +5,9 @@
|
||||
|
||||
/*
|
||||
* Verbosity is cumulative:
|
||||
* 1: navigation result (position and DOP)
|
||||
* 2: tracked channel/PRN summary
|
||||
* 1: navigation result (position and all DOP values) plus tracked
|
||||
* satellite count/PRN summary
|
||||
* 2: reserved for additional status detail
|
||||
* 3: state transitions, warnings/errors and integrity summary
|
||||
* 4: performance, NAV satellite and low-level diagnostic detail
|
||||
*
|
||||
@@ -14,7 +15,7 @@
|
||||
* levels. GPS_DEBUG_LEVEL should normally be in the range 1..4.
|
||||
*/
|
||||
#define DEBUG_STATUS 1
|
||||
#define DEBUG_CHANNELS 2
|
||||
#define DEBUG_CHANNELS DEBUG_STATUS
|
||||
#define DEBUG_EVENTS 3
|
||||
#define DEBUG_TRACE 4
|
||||
|
||||
@@ -31,6 +32,7 @@
|
||||
#define GPS_LOG_CAT_MEM "MEM"
|
||||
#define GPS_LOG_CAT_HWREG "HWREG"
|
||||
#define GPS_LOG_CAT_FUSION "FUSION"
|
||||
#define GPS_LOG_CAT_LOCATION "LOCATION"
|
||||
|
||||
// Set the current verbosity level
|
||||
#ifdef DEBUG
|
||||
@@ -0,0 +1,41 @@
|
||||
#ifndef GPS_RECEIVER_H
|
||||
#define GPS_RECEIVER_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
struct gps_nav_solution;
|
||||
|
||||
/*
|
||||
* High-level boundary around the legacy GPS acquisition/tracking subsystem.
|
||||
*
|
||||
* initialize(): resets GPS hardware/software state and installs the machine
|
||||
* interrupt handler used by the receiver.
|
||||
*
|
||||
* start_acquisition(): schedules the initial PRN acquisition work.
|
||||
*
|
||||
* process_pending_events(): foreground half of GPS interrupt processing. It
|
||||
* consumes ISR events, handles channel loss/reacquisition and timer work.
|
||||
*
|
||||
* solve_ready_epoch(): when at least four coherent channels report the same
|
||||
* nav-solve epoch, snapshots them and runs WLS. The returned timestamp is the
|
||||
* measurement epoch, not the later time at which WLS completed.
|
||||
*/
|
||||
void gps_receiver_initialize(unsigned int nav_solve_timer_ticks);
|
||||
typedef void (*gps_receiver_external_irq_handler_t)(void);
|
||||
/* Attach one application-owned PLIC source to the installed trap dispatcher.
|
||||
* The source driver remains responsible for its priority and enable state. */
|
||||
bool gps_receiver_attach_external_irq(
|
||||
unsigned int irq_id,
|
||||
gps_receiver_external_irq_handler_t handler);
|
||||
void gps_receiver_start_acquisition(void);
|
||||
void gps_receiver_process_pending_events(void);
|
||||
bool gps_receiver_solve_ready_epoch(
|
||||
struct gps_nav_solution* solution_out,
|
||||
uint64_t* measurement_timestamp_ticks_out);
|
||||
|
||||
/* Slow status clock maintained by the GPS machine-timer interrupt. */
|
||||
uint64_t gps_receiver_status_ticks(void);
|
||||
uint64_t gps_receiver_status_ticks_per_second(void);
|
||||
|
||||
#endif
|
||||
+1
-1
@@ -1,7 +1,7 @@
|
||||
#ifndef LINKED_LIST_H_INCLUDED
|
||||
#define LINKED_LIST_H_INCLUDED
|
||||
|
||||
#include "gps_ip_defines.h"
|
||||
#include "gps/gps_ip_defines.h"
|
||||
|
||||
typedef struct gps_ephem_ll {
|
||||
volatile gps_ephemeris_t* data_p;
|
||||
@@ -1,12 +0,0 @@
|
||||
#ifndef GPS_FUSION_PUBLISHER_H
|
||||
#define GPS_FUSION_PUBLISHER_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "NavEquations.h"
|
||||
|
||||
bool gps_fusion_publish_solution(const gps_nav_solution_t* solution,
|
||||
uint64_t measurement_timestamp_ticks);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,28 @@
|
||||
#ifndef GPS_SYSTEM_APP_H
|
||||
#define GPS_SYSTEM_APP_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "fusion/fusion_measurement.h"
|
||||
#include "fusion/navigation_fusion.h"
|
||||
#include "gps/NavEquations.h"
|
||||
#include "imu/imu_sample.h"
|
||||
|
||||
/* Application-owned messages and lifecycle state at the GPS/IMU/fusion
|
||||
* boundary. Solver, driver and ESKF private state remain in their modules. */
|
||||
typedef struct {
|
||||
gps_nav_solution_t gps_solution;
|
||||
fusion_gps_measurement_t gps_for_fusion;
|
||||
imu_sample_t imu_sample;
|
||||
navigation_fusion_output_t fused_solution;
|
||||
uint64_t gps_measurement_ticks;
|
||||
uint64_t last_status_tick;
|
||||
bool gps_epoch_ready;
|
||||
bool fusion_enabled;
|
||||
bool imu_initialized;
|
||||
bool fusion_initialized;
|
||||
bool imu_running;
|
||||
} gps_system_state_t;
|
||||
|
||||
#endif
|
||||
+2
-2
@@ -3,7 +3,7 @@
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "fusion_measurement.h"
|
||||
#include "imu/imu_sample.h"
|
||||
|
||||
/*
|
||||
* Sensor-independent calibration in the IMU body frame.
|
||||
@@ -28,6 +28,6 @@ typedef struct {
|
||||
void imu_calibration_default(imu_calibration_t* calibration);
|
||||
bool imu_calibration_is_valid(const imu_calibration_t* calibration);
|
||||
bool imu_calibration_apply(const imu_calibration_t* calibration,
|
||||
fusion_imu_measurement_t* measurement);
|
||||
imu_sample_t* measurement);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,18 @@
|
||||
#ifndef IMU_SAMPLE_H
|
||||
#define IMU_SAMPLE_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/*
|
||||
* Sensor-independent, calibrated inertial sample in SI units. Drivers and
|
||||
* IMU management publish this type without depending on GPS or ESKF headers.
|
||||
*/
|
||||
typedef struct {
|
||||
uint64_t timestamp_ticks;
|
||||
double accel_mps2[3];
|
||||
double gyro_rps[3];
|
||||
double temperature_c;
|
||||
uint32_t sequence;
|
||||
} imu_sample_t;
|
||||
|
||||
#endif
|
||||
+16
-13
@@ -4,9 +4,8 @@
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "fusion_measurement.h"
|
||||
#include "fusion_imu_quality.h"
|
||||
#include "imu_calibration.h"
|
||||
#include "imu/imu_calibration.h"
|
||||
#include "imu/imu_sample.h"
|
||||
|
||||
typedef enum {
|
||||
IMU_SENSOR_NONE = 0,
|
||||
@@ -16,7 +15,8 @@ typedef enum {
|
||||
} imu_sensor_backend_t;
|
||||
|
||||
typedef struct {
|
||||
/* Zero selects the active backend's safe default. */
|
||||
/* Zero selects the active backend's safe default. A non-zero target may
|
||||
* be quantized to a nearby hardware ODR; diagnostics reports both values. */
|
||||
uint32_t odr_hz;
|
||||
unsigned int accel_full_scale_g;
|
||||
unsigned int gyro_full_scale_dps;
|
||||
@@ -32,6 +32,7 @@ typedef struct {
|
||||
uint64_t queue_overflow_count;
|
||||
uint64_t estimated_missed_sample_count;
|
||||
uint64_t last_timestamp_ticks;
|
||||
uint32_t requested_odr_hz;
|
||||
uint32_t configured_odr_hz;
|
||||
uint8_t who_am_i;
|
||||
imu_sensor_backend_t backend;
|
||||
@@ -41,25 +42,27 @@ typedef struct {
|
||||
} imu_sensor_diagnostics_t;
|
||||
|
||||
void imu_sensor_default_config(imu_sensor_config_t* config);
|
||||
|
||||
/* Probe the compiled backend(s), select one, validate the requested ODR/range
|
||||
* and prepare calibration. This does not enable data-ready interrupts. */
|
||||
bool imu_sensor_init(const imu_sensor_config_t* config);
|
||||
|
||||
/* Configure streaming and enable BSP_PMOD_IRQ. The application must attach
|
||||
* imu_sensor_irq_handler to the active machine-interrupt router first. */
|
||||
bool imu_sensor_start(void);
|
||||
void imu_sensor_stop(void);
|
||||
|
||||
/* Interrupt backend hook. Safe no-op for a foreground-polled backend. */
|
||||
/* PLIC/ISR half: read and timestamp one raw sample, then enqueue it. */
|
||||
void imu_sensor_irq_handler(void);
|
||||
|
||||
/* Foreground acquisition hook. Safe no-op for an interrupt backend. */
|
||||
/* Foreground half. Safe no-op for the current interrupt-driven backends. */
|
||||
void imu_sensor_service(void);
|
||||
bool imu_sensor_requires_foreground_service(void);
|
||||
|
||||
bool imu_sensor_take_measurement(fusion_imu_measurement_t* measurement_out);
|
||||
/* Pop the oldest sample, convert to SI/common axes and apply the selected
|
||||
* backend's calibration. No averaging or motion decision occurs here. */
|
||||
bool imu_sensor_take_measurement(imu_sample_t* measurement_out);
|
||||
void imu_sensor_get_diagnostics(imu_sensor_diagnostics_t* diagnostics_out);
|
||||
imu_sensor_backend_t imu_sensor_active_backend(void);
|
||||
const char* imu_sensor_backend_name(imu_sensor_backend_t backend);
|
||||
bool imu_sensor_quality_profile_for_backend(
|
||||
imu_sensor_backend_t backend,
|
||||
fusion_imu_quality_profile_t* profile_out);
|
||||
bool imu_sensor_active_quality_profile(
|
||||
fusion_imu_quality_profile_t* profile_out);
|
||||
|
||||
#endif
|
||||
@@ -1,33 +0,0 @@
|
||||
#ifndef NAVIGATION_FUSION_H
|
||||
#define NAVIGATION_FUSION_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#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
|
||||
@@ -0,0 +1,30 @@
|
||||
#include "adapters/gps_fusion_publisher.h"
|
||||
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
bool gps_fusion_build_measurement(
|
||||
const gps_nav_solution_t* solution,
|
||||
uint64_t measurement_timestamp_ticks,
|
||||
fusion_gps_measurement_t* measurement_out)
|
||||
{
|
||||
if((solution == NULL) || (measurement_timestamp_ticks == 0u) ||
|
||||
(measurement_out == NULL))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
memset(measurement_out, 0, sizeof(*measurement_out));
|
||||
measurement_out->timestamp_ticks = measurement_timestamp_ticks;
|
||||
measurement_out->latitude_deg = solution->latitude_deg;
|
||||
measurement_out->longitude_deg = solution->longitude_deg;
|
||||
measurement_out->height_m = solution->height_m;
|
||||
memcpy(measurement_out->covariance_enu_m2, solution->covariance_enu_m2,
|
||||
sizeof(measurement_out->covariance_enu_m2));
|
||||
measurement_out->hacc_m = solution->hacc_m;
|
||||
measurement_out->vacc_m = solution->vacc_m;
|
||||
measurement_out->residual_rms_m = solution->residual_rms_m;
|
||||
measurement_out->chi_square_ratio = solution->chi_square_ratio;
|
||||
measurement_out->used_satellites = solution->used_satellite_count;
|
||||
measurement_out->solution_valid = solution->solution_valid;
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,112 @@
|
||||
#include "adapters/gps_system_uart.h"
|
||||
|
||||
#include <bsp/soc.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#include "gps/gps_macros.h"
|
||||
#include "gps/gps_receiver.h"
|
||||
#include "imu/imu_sensor.h"
|
||||
|
||||
void gps_system_uart_publish_location(
|
||||
const char* source,
|
||||
double latitude_deg,
|
||||
double longitude_deg,
|
||||
double height_m,
|
||||
const gps_nav_solution_t* gps)
|
||||
{
|
||||
if((source == NULL) || (gps == NULL))
|
||||
{
|
||||
return;
|
||||
}
|
||||
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION,
|
||||
"source=%s lat=%.20lf lon=%.20lf height=%.3lf "
|
||||
"gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf "
|
||||
"tracked=%u used=%u\r\n",
|
||||
source, latitude_deg, longitude_deg, height_m,
|
||||
gps->gdop, gps->pdop, gps->hdop, gps->vdop, gps->tdop,
|
||||
gps->tracked_satellite_count, gps->used_satellite_count);
|
||||
}
|
||||
|
||||
void gps_system_uart_publish_startup(
|
||||
bool fusion_enabled,
|
||||
bool fusion_initialized,
|
||||
bool imu_initialized,
|
||||
bool imu_running)
|
||||
{
|
||||
imu_sensor_diagnostics_t imu;
|
||||
imu_sensor_get_diagnostics(&imu);
|
||||
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
|
||||
"init eskf_enabled=%u eskf_ready=%u imu=%u sensor=%s who=0x%02x "
|
||||
"running=%u target_hz=%lu actual_hz=%lu nav_ms=%u timebase_hz=%lu\r\n",
|
||||
fusion_enabled ? 1u : 0u,
|
||||
fusion_initialized ? 1u : 0u,
|
||||
imu_initialized ? 1u : 0u,
|
||||
imu_sensor_backend_name(imu.backend),
|
||||
(unsigned int)imu.who_am_i,
|
||||
imu_running ? 1u : 0u,
|
||||
(unsigned long)imu.requested_odr_hz,
|
||||
(unsigned long)imu.configured_odr_hz,
|
||||
GPS_NAV_SOLVE_PERIOD_MS,
|
||||
(unsigned long)BSP_CLINT_TIMEBASE_HZ);
|
||||
}
|
||||
|
||||
void gps_system_uart_publish_periodic_fusion_status(
|
||||
navigation_fusion_output_t* output,
|
||||
uint64_t* last_status_tick)
|
||||
{
|
||||
if((output == NULL) || (last_status_tick == NULL))
|
||||
{
|
||||
return;
|
||||
}
|
||||
uint64_t now = gps_receiver_status_ticks();
|
||||
if((now - *last_status_tick) < gps_receiver_status_ticks_per_second())
|
||||
{
|
||||
return;
|
||||
}
|
||||
*last_status_tick = now;
|
||||
|
||||
imu_sensor_diagnostics_t imu;
|
||||
imu_sensor_get_diagnostics(&imu);
|
||||
if(!navigation_fusion_get_output(output))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if(output->global_valid)
|
||||
{
|
||||
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
|
||||
"state=%s lat=%.10lf lon=%.10lf height=%.3lf "
|
||||
"speed=%.3lf nis=%.3lf gps=%lu/%lu recoveries=%lu "
|
||||
"zupt=%lu replay=%lu "
|
||||
"sigma=%.2lf,%.2lf,%.2lf imu=%lu fused_imu=%lu "
|
||||
"overflow=%lu missed=%lu\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,
|
||||
(unsigned long)output->gps_position_recoveries,
|
||||
(unsigned long)output->zero_update_count,
|
||||
(unsigned long)output->replay_count,
|
||||
output->position_std_m[0], output->position_std_m[1],
|
||||
output->position_std_m[2],
|
||||
(unsigned long)imu.sample_count,
|
||||
(unsigned long)output->imu_processed_count,
|
||||
(unsigned long)imu.queue_overflow_count,
|
||||
(unsigned long)imu.estimated_missed_sample_count);
|
||||
return;
|
||||
}
|
||||
|
||||
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
|
||||
"state=%s calibration_samples=%u gps=%lu/%lu recoveries=%lu "
|
||||
"imu=%lu fused_imu=%lu overflow=%lu missed=%lu\r\n",
|
||||
navigation_fusion_state_name(output->engine_state),
|
||||
output->calibration_sample_count,
|
||||
(unsigned long)output->gps_accepted,
|
||||
(unsigned long)output->gps_nis_rejected,
|
||||
(unsigned long)output->gps_position_recoveries,
|
||||
(unsigned long)imu.sample_count,
|
||||
(unsigned long)output->imu_processed_count,
|
||||
(unsigned long)imu.queue_overflow_count,
|
||||
(unsigned long)imu.estimated_missed_sample_count);
|
||||
}
|
||||
+2
-2
@@ -1,4 +1,4 @@
|
||||
#include "imu_sensor.h"
|
||||
#include "adapters/imu_fusion_profile.h"
|
||||
|
||||
#include <stddef.h>
|
||||
|
||||
@@ -13,7 +13,7 @@ static void set_all(double values[3], double value)
|
||||
}
|
||||
}
|
||||
|
||||
bool imu_sensor_quality_profile_for_backend(
|
||||
bool imu_fusion_quality_profile_for_backend(
|
||||
imu_sensor_backend_t backend,
|
||||
fusion_imu_quality_profile_t* profile_out)
|
||||
{
|
||||
+2
-2
@@ -1,4 +1,4 @@
|
||||
#include "st_lsm6dsox.h"
|
||||
#include "drivers/imu/st_lsm6dsox.h"
|
||||
|
||||
#include <bsp/mmio.h>
|
||||
#include <bsp/plic.h>
|
||||
@@ -437,7 +437,7 @@ void st_lsm6dsox_service(void)
|
||||
}
|
||||
|
||||
bool st_lsm6dsox_take_measurement(
|
||||
fusion_imu_measurement_t* measurement_out)
|
||||
imu_sample_t* measurement_out)
|
||||
{
|
||||
if(measurement_out == NULL)
|
||||
{
|
||||
+2
-2
@@ -1,4 +1,4 @@
|
||||
#include "tdk_icm42688.h"
|
||||
#include "drivers/imu/tdk_icm42688.h"
|
||||
|
||||
#include <bsp/mmio.h>
|
||||
#include <bsp/plic.h>
|
||||
@@ -342,7 +342,7 @@ void tdk_icm42688_irq_handler(void)
|
||||
__atomic_fetch_add(&diagnostics.sample_count, 1u, __ATOMIC_RELAXED);
|
||||
}
|
||||
|
||||
bool tdk_icm42688_take_measurement(fusion_imu_measurement_t* measurement_out)
|
||||
bool tdk_icm42688_take_measurement(imu_sample_t* measurement_out)
|
||||
{
|
||||
if(measurement_out == NULL)
|
||||
{
|
||||
@@ -1,4 +1,4 @@
|
||||
#include "eskf.h"
|
||||
#include "fusion/eskf.h"
|
||||
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
@@ -325,21 +325,41 @@ bool eskf_initialize(eskf_t* filter,
|
||||
|
||||
static bool eskf_propagate_step(eskf_t* filter,
|
||||
const eskf_config_t* config,
|
||||
const double accel_mps2[3],
|
||||
const double gyro_rps[3],
|
||||
const double start_accel_mps2[3],
|
||||
const double start_gyro_rps[3],
|
||||
const double end_accel_mps2[3],
|
||||
const double end_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];
|
||||
specific_force[i] =
|
||||
0.5 * (start_accel_mps2[i] + end_accel_mps2[i]) -
|
||||
filter->accel_bias_mps2[i];
|
||||
angular_rate[i] =
|
||||
0.5 * (start_gyro_rps[i] + end_gyro_rps[i]) -
|
||||
filter->gyro_bias_rps[i];
|
||||
filter->last_unbiased_gyro_rps[i] = angular_rate[i];
|
||||
}
|
||||
|
||||
double half_rotation_vector[3] = {
|
||||
0.5 * angular_rate[0] * dt_s,
|
||||
0.5 * angular_rate[1] * dt_s,
|
||||
0.5 * angular_rate[2] * dt_s
|
||||
};
|
||||
double half_delta_quaternion[4];
|
||||
double midpoint_quaternion[4];
|
||||
rotation_vector_quaternion(half_rotation_vector, half_delta_quaternion);
|
||||
quaternion_multiply(filter->body_to_nav_quaternion, half_delta_quaternion,
|
||||
midpoint_quaternion);
|
||||
if(!quaternion_normalize(midpoint_quaternion))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double rotation[3][3];
|
||||
quaternion_rotation_matrix(filter->body_to_nav_quaternion, rotation);
|
||||
quaternion_rotation_matrix(midpoint_quaternion, rotation);
|
||||
double linear_accel[3] = {0.0, 0.0, -config->gravity_mps2};
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
@@ -412,22 +432,36 @@ static bool eskf_propagate_step(eskf_t* filter,
|
||||
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++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
double coefficient = transition[row][k];
|
||||
if(coefficient == 0.0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
intermediate[row][col] +=
|
||||
transition[row][k] * filter->covariance[k][col];
|
||||
coefficient * 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++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
double value = intermediate[row][k];
|
||||
if(value == 0.0)
|
||||
{
|
||||
propagated[row][col] += intermediate[row][k] * transition[col][k];
|
||||
continue;
|
||||
}
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
double coefficient = transition[col][k];
|
||||
if(coefficient != 0.0)
|
||||
{
|
||||
propagated[row][col] += value * coefficient;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -459,14 +493,20 @@ static bool eskf_propagate_step(eskf_t* filter,
|
||||
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)
|
||||
bool eskf_propagate_interval(
|
||||
eskf_t* filter,
|
||||
const eskf_config_t* config,
|
||||
const double start_accel_mps2[3],
|
||||
const double start_gyro_rps[3],
|
||||
const double end_accel_mps2[3],
|
||||
const double end_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) ||
|
||||
!vector_is_finite(start_accel_mps2, 3u) ||
|
||||
!vector_is_finite(start_gyro_rps, 3u) ||
|
||||
!vector_is_finite(end_accel_mps2, 3u) ||
|
||||
!vector_is_finite(end_gyro_rps, 3u) ||
|
||||
!isfinite(dt_s) || (dt_s < 0.0) ||
|
||||
!(config->max_propagation_step_s > 0.0))
|
||||
{
|
||||
@@ -484,7 +524,28 @@ bool eskf_propagate(eskf_t* filter,
|
||||
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))
|
||||
double start_fraction = (double)step / (double)steps;
|
||||
double end_fraction = (double)(step + 1u) / (double)steps;
|
||||
double step_start_accel[3];
|
||||
double step_start_gyro[3];
|
||||
double step_end_accel[3];
|
||||
double step_end_gyro[3];
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
double accel_delta = end_accel_mps2[axis] - start_accel_mps2[axis];
|
||||
double gyro_delta = end_gyro_rps[axis] - start_gyro_rps[axis];
|
||||
step_start_accel[axis] =
|
||||
start_accel_mps2[axis] + start_fraction * accel_delta;
|
||||
step_end_accel[axis] =
|
||||
start_accel_mps2[axis] + end_fraction * accel_delta;
|
||||
step_start_gyro[axis] =
|
||||
start_gyro_rps[axis] + start_fraction * gyro_delta;
|
||||
step_end_gyro[axis] =
|
||||
start_gyro_rps[axis] + end_fraction * gyro_delta;
|
||||
}
|
||||
if(!eskf_propagate_step(filter, config,
|
||||
step_start_accel, step_start_gyro,
|
||||
step_end_accel, step_end_gyro, step_s))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
@@ -492,6 +553,17 @@ bool eskf_propagate(eskf_t* filter,
|
||||
return true;
|
||||
}
|
||||
|
||||
bool eskf_propagate(eskf_t* filter,
|
||||
const eskf_config_t* config,
|
||||
const double accel_mps2[3],
|
||||
const double gyro_rps[3],
|
||||
double dt_s)
|
||||
{
|
||||
return eskf_propagate_interval(filter, config,
|
||||
accel_mps2, gyro_rps,
|
||||
accel_mps2, gyro_rps, dt_s);
|
||||
}
|
||||
|
||||
static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STATE_DIM])
|
||||
{
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
@@ -538,21 +610,35 @@ static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STAT
|
||||
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++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
double coefficient = reset[row][k];
|
||||
if(coefficient == 0.0)
|
||||
{
|
||||
intermediate[row][col] += reset[row][k] * filter->covariance[k][col];
|
||||
continue;
|
||||
}
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
intermediate[row][col] += coefficient * 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++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
double value = intermediate[row][k];
|
||||
if(value == 0.0)
|
||||
{
|
||||
reset_covariance[row][col] += intermediate[row][k] * reset[col][k];
|
||||
continue;
|
||||
}
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
double coefficient = reset[col][k];
|
||||
if(coefficient != 0.0)
|
||||
{
|
||||
reset_covariance[row][col] += value * coefficient;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -646,12 +732,48 @@ static bool eskf_selected_update(eskf_t* filter,
|
||||
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++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
double coefficient = joseph_left[row][k];
|
||||
if(coefficient == 0.0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
intermediate[row][col] +=
|
||||
joseph_left[row][k] * filter->covariance[k][col];
|
||||
coefficient * filter->covariance[k][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
{
|
||||
double value = intermediate[row][k];
|
||||
if(value == 0.0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
double coefficient = joseph_left[col][k];
|
||||
if(coefficient != 0.0)
|
||||
{
|
||||
updated_covariance[row][col] += value * coefficient;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
double gain_times_covariance[ESKF_ERROR_STATE_DIM][3] = {{0.0}};
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int second = 0 ; second < 3u ; second++)
|
||||
{
|
||||
for(unsigned int first = 0 ; first < 3u ; first++)
|
||||
{
|
||||
gain_times_covariance[row][second] += gain[row][first] *
|
||||
measurement_covariance[first * 3u + second];
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -659,18 +781,10 @@ static bool eskf_selected_update(eskf_t* filter,
|
||||
{
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
for(unsigned int second = 0 ; second < 3u ; second++)
|
||||
{
|
||||
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];
|
||||
}
|
||||
gain_times_covariance[row][second] * gain[col][second];
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -729,6 +843,50 @@ eskf_update_result_t eskf_position_update(
|
||||
return ESKF_UPDATE_ACCEPTED;
|
||||
}
|
||||
|
||||
bool eskf_reset_position(
|
||||
eskf_t* filter,
|
||||
const double position_m[3],
|
||||
const double covariance_m2[9])
|
||||
{
|
||||
if((filter == NULL) || !filter->initialized ||
|
||||
!vector_is_finite(position_m, 3u) ||
|
||||
!vector_is_finite(covariance_m2, 9u))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
/* inverse_symmetric3() also rejects singular/non-positive-definite input. */
|
||||
double unused_inverse[9];
|
||||
if(!inverse_symmetric3(covariance_m2, unused_inverse))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
memcpy(filter->position_m, position_m, 3u * sizeof(double));
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
unsigned int state = ESKF_POSITION_INDEX + axis;
|
||||
for(unsigned int other = 0u ; other < ESKF_ERROR_STATE_DIM ; other++)
|
||||
{
|
||||
filter->covariance[state][other] = 0.0;
|
||||
filter->covariance[other][state] = 0.0;
|
||||
}
|
||||
}
|
||||
for(unsigned int row = 0u ; row < 3u ; row++)
|
||||
{
|
||||
for(unsigned int col = 0u ; col < 3u ; col++)
|
||||
{
|
||||
filter->covariance[ESKF_POSITION_INDEX + row]
|
||||
[ESKF_POSITION_INDEX + col] =
|
||||
covariance_m2[row * 3u + col];
|
||||
}
|
||||
}
|
||||
covariance_symmetrize_and_floor(filter, 1.0e-12);
|
||||
filter->last_position_nis = NAN;
|
||||
filter->position_update_accepted_count++;
|
||||
return eskf_state_is_finite(filter);
|
||||
}
|
||||
|
||||
bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps)
|
||||
{
|
||||
if((filter == NULL) || !filter->initialized ||
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
#include "fusion_bus.h"
|
||||
#include "fusion/fusion_bus.h"
|
||||
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
+273
-34
@@ -1,4 +1,4 @@
|
||||
#include "fusion_engine.h"
|
||||
#include "fusion/fusion_engine.h"
|
||||
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
@@ -123,16 +123,26 @@ void fusion_engine_default_config(fusion_engine_config_t* config,
|
||||
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;
|
||||
/* PX4 EKF2 fuses aligned zero velocity at 5 Hz with 0.2 m/s sigma. */
|
||||
config->zero_update_period_s = 0.20;
|
||||
config->zero_velocity_std_mps = 0.20;
|
||||
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_motion_window_s = 1.0;
|
||||
config->gnss_moving_confirmations = 2u;
|
||||
config->gnss_still_confirmations = 2u;
|
||||
/* A hard innovation gate protects against isolated GNSS outliers, but it
|
||||
* must not permanently lock the filter to a bad startup position. After a
|
||||
* full second of mutually continuous rejected fixes, re-anchor position
|
||||
* while preserving inertial velocity, attitude and bias states. */
|
||||
config->position_recovery_timeout_s = 7.0;
|
||||
config->position_recovery_max_speed_mps = 100.0;
|
||||
config->position_recovery_sigma_multiplier = 5.0;
|
||||
config->position_recovery_min_samples = 3u;
|
||||
}
|
||||
|
||||
bool fusion_engine_apply_imu_quality(
|
||||
@@ -178,8 +188,17 @@ static bool config_valid(const fusion_engine_config_t* config)
|
||||
isfinite(config->zero_velocity_std_mps) &&
|
||||
(config->zero_velocity_std_mps > 0.0) &&
|
||||
finite_values(config->zero_gyro_std_rps, 3u) &&
|
||||
isfinite(config->gnss_motion_window_s) &&
|
||||
(config->gnss_motion_window_s > 0.0) &&
|
||||
(config->gnss_moving_confirmations != 0u) &&
|
||||
(config->gnss_still_confirmations != 0u);
|
||||
(config->gnss_still_confirmations != 0u) &&
|
||||
isfinite(config->position_recovery_timeout_s) &&
|
||||
(config->position_recovery_timeout_s > 0.0) &&
|
||||
isfinite(config->position_recovery_max_speed_mps) &&
|
||||
(config->position_recovery_max_speed_mps > 0.0) &&
|
||||
isfinite(config->position_recovery_sigma_multiplier) &&
|
||||
(config->position_recovery_sigma_multiplier > 0.0) &&
|
||||
(config->position_recovery_min_samples >= 2u);
|
||||
}
|
||||
|
||||
bool fusion_engine_init(fusion_engine_t* engine,
|
||||
@@ -322,26 +341,32 @@ static void history_drop_through(fusion_engine_t* engine, uint64_t timestamp_tic
|
||||
|
||||
static bool runtime_propagate_to(fusion_engine_t* engine,
|
||||
fusion_runtime_t* runtime,
|
||||
uint64_t timestamp_ticks,
|
||||
const fusion_imu_measurement_t* input)
|
||||
const fusion_imu_measurement_t* end_input)
|
||||
{
|
||||
if((runtime == NULL) || (input == NULL) || !runtime->filter.initialized ||
|
||||
(timestamp_ticks < runtime->filter_timestamp_ticks))
|
||||
if((runtime == NULL) || (end_input == NULL) ||
|
||||
!runtime->filter.initialized || !runtime->have_held_imu ||
|
||||
(runtime->filter_timestamp_ticks != runtime->held_imu.timestamp_ticks) ||
|
||||
(end_input->timestamp_ticks < runtime->filter_timestamp_ticks))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(timestamp_ticks == runtime->filter_timestamp_ticks)
|
||||
if(end_input->timestamp_ticks == runtime->filter_timestamp_ticks)
|
||||
{
|
||||
runtime->held_imu = *end_input;
|
||||
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))
|
||||
end_input->timestamp_ticks -
|
||||
runtime->filter_timestamp_ticks);
|
||||
if(!eskf_propagate_interval(
|
||||
&runtime->filter, &engine->config.eskf,
|
||||
runtime->held_imu.accel_mps2, runtime->held_imu.gyro_rps,
|
||||
end_input->accel_mps2, end_input->gyro_rps, dt_s))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
runtime->filter_timestamp_ticks = timestamp_ticks;
|
||||
runtime->filter_timestamp_ticks = end_input->timestamp_ticks;
|
||||
runtime->held_imu = *end_input;
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -364,7 +389,8 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
|
||||
}
|
||||
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))
|
||||
fusion_imu_measurement_t start_measurement = runtime->held_imu;
|
||||
if(!runtime_propagate_to(engine, runtime, measurement))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
@@ -388,31 +414,82 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
|
||||
runtime->gnss_moving_count = 0u;
|
||||
runtime->gnss_still_count = 0u;
|
||||
}
|
||||
runtime->held_imu = *measurement;
|
||||
|
||||
runtime->vehicle_at_rest = measurement->vehicle_at_rest;
|
||||
bool at_rest_continuing = measurement->vehicle_at_rest &&
|
||||
start_measurement.vehicle_at_rest;
|
||||
if(measurement->vehicle_at_rest)
|
||||
{
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
double mean_gyro = 0.5 *
|
||||
(start_measurement.gyro_rps[axis] + measurement->gyro_rps[axis]);
|
||||
runtime->zero_gyro_integral_rad[axis] += mean_gyro * motion_dt_s;
|
||||
}
|
||||
runtime->zero_gyro_duration_s += motion_dt_s;
|
||||
}
|
||||
else
|
||||
{
|
||||
memset(runtime->zero_gyro_integral_rad, 0,
|
||||
sizeof(runtime->zero_gyro_integral_rad));
|
||||
runtime->zero_gyro_duration_s = 0.0;
|
||||
}
|
||||
uint64_t zero_period_ticks =
|
||||
seconds_to_ticks(engine, engine->config.zero_update_period_s);
|
||||
if(fusion_motion_allows_zero_update(&runtime->motion) &&
|
||||
if(at_rest_continuing &&
|
||||
((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))
|
||||
double mean_gyro_rps[3];
|
||||
if(!(runtime->zero_gyro_duration_s > 0.0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
mean_gyro_rps[axis] = runtime->zero_gyro_integral_rad[axis] /
|
||||
runtime->zero_gyro_duration_s;
|
||||
}
|
||||
if(!eskf_zero_velocity_update(
|
||||
&runtime->filter, engine->config.zero_velocity_std_mps) ||
|
||||
!eskf_zero_angular_rate_update(
|
||||
&runtime->filter, mean_gyro_rps, engine->config.zero_gyro_std_rps))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
memset(runtime->zero_gyro_integral_rad, 0,
|
||||
sizeof(runtime->zero_gyro_integral_rad));
|
||||
runtime->zero_gyro_duration_s = 0.0;
|
||||
runtime->last_zero_update_ticks = measurement->timestamp_ticks;
|
||||
if(count_zero_updates)
|
||||
{
|
||||
engine->diagnostics.zero_velocity_update_count++;
|
||||
engine->diagnostics.zero_gyro_update_count++;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static fusion_imu_measurement_t interpolate_imu(
|
||||
const fusion_imu_measurement_t* start,
|
||||
const fusion_imu_measurement_t* end,
|
||||
uint64_t timestamp_ticks)
|
||||
{
|
||||
fusion_imu_measurement_t interpolated = *start;
|
||||
interpolated.timestamp_ticks = timestamp_ticks;
|
||||
double fraction = (double)(timestamp_ticks - start->timestamp_ticks) /
|
||||
(double)(end->timestamp_ticks - start->timestamp_ticks);
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
interpolated.accel_mps2[axis] = start->accel_mps2[axis] + fraction *
|
||||
(end->accel_mps2[axis] - start->accel_mps2[axis]);
|
||||
interpolated.gyro_rps[axis] = start->gyro_rps[axis] + fraction *
|
||||
(end->gyro_rps[axis] - start->gyro_rps[axis]);
|
||||
}
|
||||
interpolated.temperature_c = start->temperature_c + fraction *
|
||||
(end->temperature_c - start->temperature_c);
|
||||
return interpolated;
|
||||
}
|
||||
|
||||
static bool finish_calibration(fusion_engine_t* engine,
|
||||
const fusion_imu_measurement_t* last_measurement)
|
||||
{
|
||||
@@ -461,6 +538,8 @@ static bool finish_calibration(fusion_engine_t* engine,
|
||||
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.last_position_fuse_ticks = last_measurement->timestamp_ticks;
|
||||
engine->current.vehicle_at_rest = last_measurement->vehicle_at_rest;
|
||||
engine->current.gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
|
||||
if(engine->have_initial_position)
|
||||
{
|
||||
@@ -514,8 +593,13 @@ static bool process_calibration_imu(fusion_engine_t* engine,
|
||||
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)
|
||||
const fusion_position_measurement_t* measurement,
|
||||
bool* window_complete_out)
|
||||
{
|
||||
if(window_complete_out != NULL)
|
||||
{
|
||||
*window_complete_out = false;
|
||||
}
|
||||
if(!runtime->have_accepted_position ||
|
||||
(measurement->timestamp_ticks <= runtime->last_accepted_position_ticks))
|
||||
{
|
||||
@@ -527,6 +611,14 @@ static fusion_gnss_motion_evidence_t position_motion_evidence(
|
||||
{
|
||||
return FUSION_GNSS_MOTION_UNKNOWN;
|
||||
}
|
||||
if(dt_s < engine->config.gnss_motion_window_s)
|
||||
{
|
||||
return FUSION_GNSS_MOTION_UNKNOWN;
|
||||
}
|
||||
if(window_complete_out != NULL)
|
||||
{
|
||||
*window_complete_out = true;
|
||||
}
|
||||
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] +
|
||||
@@ -556,6 +648,114 @@ static fusion_gnss_motion_evidence_t position_motion_evidence(
|
||||
return FUSION_GNSS_MOTION_STILL;
|
||||
}
|
||||
|
||||
static void position_recovery_clear(fusion_engine_t* engine)
|
||||
{
|
||||
engine->position_recovery_active = false;
|
||||
engine->position_recovery_start_ticks = 0u;
|
||||
engine->position_recovery_consistent_count = 0u;
|
||||
memset(&engine->position_recovery_candidate, 0,
|
||||
sizeof(engine->position_recovery_candidate));
|
||||
}
|
||||
|
||||
static void position_recovery_begin(
|
||||
fusion_engine_t* engine,
|
||||
const fusion_position_measurement_t* measurement)
|
||||
{
|
||||
engine->position_recovery_candidate = *measurement;
|
||||
engine->position_recovery_start_ticks = measurement->timestamp_ticks;
|
||||
engine->position_recovery_consistent_count = 1u;
|
||||
engine->position_recovery_active = true;
|
||||
}
|
||||
|
||||
static bool position_recovery_is_continuous(
|
||||
const fusion_engine_t* engine,
|
||||
const fusion_position_measurement_t* previous,
|
||||
const fusion_position_measurement_t* current)
|
||||
{
|
||||
if(current->timestamp_ticks <= previous->timestamp_ticks)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double dt_s = ticks_to_seconds(
|
||||
engine, current->timestamp_ticks - previous->timestamp_ticks);
|
||||
double displacement_squared = 0.0;
|
||||
double combined_variance = 0.0;
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
double delta = current->position_enu_m[axis] -
|
||||
previous->position_enu_m[axis];
|
||||
displacement_squared += delta * delta;
|
||||
double variance = current->covariance_enu_m2[axis * 3u + axis] +
|
||||
previous->covariance_enu_m2[axis * 3u + axis];
|
||||
if(!(variance > 0.0) || !isfinite(variance))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
combined_variance += variance;
|
||||
}
|
||||
double allowed_displacement =
|
||||
engine->config.position_recovery_max_speed_mps * dt_s +
|
||||
engine->config.position_recovery_sigma_multiplier *
|
||||
sqrt(combined_variance);
|
||||
return isfinite(displacement_squared) &&
|
||||
(displacement_squared <= square(allowed_displacement));
|
||||
}
|
||||
|
||||
static bool position_recovery_ready(
|
||||
fusion_engine_t* engine,
|
||||
const fusion_runtime_t* runtime,
|
||||
const fusion_position_measurement_t* measurement)
|
||||
{
|
||||
if(!engine->position_recovery_active)
|
||||
{
|
||||
position_recovery_begin(engine, measurement);
|
||||
return false;
|
||||
}
|
||||
if(!position_recovery_is_continuous(
|
||||
engine, &engine->position_recovery_candidate, measurement))
|
||||
{
|
||||
/* A discontinuity is an isolated outlier or a new candidate track; it
|
||||
* must earn a complete recovery interval of its own. */
|
||||
position_recovery_begin(engine, measurement);
|
||||
return false;
|
||||
}
|
||||
engine->position_recovery_candidate = *measurement;
|
||||
if(engine->position_recovery_consistent_count < UINT32_MAX)
|
||||
{
|
||||
engine->position_recovery_consistent_count++;
|
||||
}
|
||||
if(engine->position_recovery_consistent_count <
|
||||
engine->config.position_recovery_min_samples)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(measurement->timestamp_ticks < runtime->last_position_fuse_ticks)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
uint64_t elapsed_ticks = measurement->timestamp_ticks -
|
||||
runtime->last_position_fuse_ticks;
|
||||
return elapsed_ticks >= seconds_to_ticks(
|
||||
engine, engine->config.position_recovery_timeout_s);
|
||||
}
|
||||
|
||||
static void runtime_record_recovered_position(
|
||||
fusion_runtime_t* runtime,
|
||||
const fusion_position_measurement_t* measurement)
|
||||
{
|
||||
runtime->have_accepted_position = true;
|
||||
runtime->last_accepted_position_ticks = measurement->timestamp_ticks;
|
||||
runtime->last_position_fuse_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));
|
||||
runtime->gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
|
||||
runtime->gnss_moving_count = 0u;
|
||||
runtime->gnss_still_count = 0u;
|
||||
}
|
||||
|
||||
static eskf_update_result_t runtime_apply_position(
|
||||
fusion_engine_t* engine,
|
||||
fusion_runtime_t* runtime,
|
||||
@@ -567,8 +767,11 @@ static eskf_update_result_t runtime_apply_position(
|
||||
measurement->position_enu_m, measurement->covariance_enu_m2, nis_out);
|
||||
if(result == ESKF_UPDATE_ACCEPTED)
|
||||
{
|
||||
runtime->last_position_fuse_ticks = measurement->timestamp_ticks;
|
||||
bool motion_window_complete = false;
|
||||
fusion_gnss_motion_evidence_t raw_evidence =
|
||||
position_motion_evidence(engine, runtime, measurement);
|
||||
position_motion_evidence(engine, runtime, measurement,
|
||||
&motion_window_complete);
|
||||
if(raw_evidence == FUSION_GNSS_MOTION_MOVING)
|
||||
{
|
||||
runtime->gnss_still_count = 0u;
|
||||
@@ -593,13 +796,19 @@ static eskf_update_result_t runtime_apply_position(
|
||||
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));
|
||||
/* Keep a long-enough displacement baseline. At a 200 ms GNSS period,
|
||||
* replacing the reference on every fix would classify statistically
|
||||
* inconclusive 10--20 cm steps as stationary evidence. */
|
||||
if(!runtime->have_accepted_position || motion_window_complete)
|
||||
{
|
||||
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;
|
||||
}
|
||||
@@ -617,11 +826,13 @@ static bool process_position_now(fusion_engine_t* engine,
|
||||
}
|
||||
fusion_runtime_t revised = engine->checkpoint;
|
||||
uint64_t replayed = 0u;
|
||||
const fusion_imu_measurement_t* next_imu = NULL;
|
||||
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)
|
||||
{
|
||||
next_imu = imu;
|
||||
break;
|
||||
}
|
||||
if(!runtime_apply_imu(engine, &revised, imu, false))
|
||||
@@ -633,9 +844,20 @@ static bool process_position_now(fusion_engine_t* engine,
|
||||
}
|
||||
if(revised.filter_timestamp_ticks < measurement->timestamp_ticks)
|
||||
{
|
||||
if(!revised.have_held_imu ||
|
||||
!runtime_propagate_to(engine, &revised,
|
||||
measurement->timestamp_ticks, &revised.held_imu))
|
||||
if(!revised.have_held_imu)
|
||||
{
|
||||
engine->state = FUSION_ENGINE_FAULT;
|
||||
return false;
|
||||
}
|
||||
fusion_imu_measurement_t endpoint = revised.held_imu;
|
||||
endpoint.timestamp_ticks = measurement->timestamp_ticks;
|
||||
if((next_imu != NULL) &&
|
||||
(next_imu->timestamp_ticks > revised.held_imu.timestamp_ticks))
|
||||
{
|
||||
endpoint = interpolate_imu(&revised.held_imu, next_imu,
|
||||
measurement->timestamp_ticks);
|
||||
}
|
||||
if(!runtime_propagate_to(engine, &revised, &endpoint))
|
||||
{
|
||||
engine->state = FUSION_ENGINE_FAULT;
|
||||
return false;
|
||||
@@ -649,6 +871,23 @@ static bool process_position_now(fusion_engine_t* engine,
|
||||
return false;
|
||||
}
|
||||
if(result == ESKF_UPDATE_ACCEPTED)
|
||||
{
|
||||
position_recovery_clear(engine);
|
||||
}
|
||||
else if((result == ESKF_UPDATE_REJECTED_NIS) &&
|
||||
position_recovery_ready(engine, &revised, measurement) &&
|
||||
eskf_reset_position(&revised.filter, measurement->position_enu_m,
|
||||
measurement->covariance_enu_m2))
|
||||
{
|
||||
/* This is a deliberate position-state reset, not a Kalman update with
|
||||
* a disabled gate. The independent IMU-derived attitude and bias state
|
||||
* survive, while invalid position cross-covariances are discarded. */
|
||||
runtime_record_recovered_position(&revised, measurement);
|
||||
engine->diagnostics.position_recovery_count++;
|
||||
position_recovery_clear(engine);
|
||||
result = ESKF_UPDATE_ACCEPTED;
|
||||
}
|
||||
if(result == ESKF_UPDATE_ACCEPTED)
|
||||
{
|
||||
engine->diagnostics.position_accepted++;
|
||||
}
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
#include "fusion_gps_adapter.h"
|
||||
#include "fusion/fusion_gps_adapter.h"
|
||||
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
#include "fusion_motion.h"
|
||||
#include "fusion/fusion_motion.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
@@ -0,0 +1,144 @@
|
||||
#include "fusion/fusion_rest_detector.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
static double difference_norm3(const double current[3], const double previous[3])
|
||||
{
|
||||
double sum = 0.0;
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
double difference = current[axis] - previous[axis];
|
||||
sum += difference * difference;
|
||||
}
|
||||
return sqrt(sum);
|
||||
}
|
||||
|
||||
void fusion_rest_detector_default_config(fusion_rest_detector_config_t* config)
|
||||
{
|
||||
if(config == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
/* Same high-frequency vibration metric and limits used by PX4's current
|
||||
* VehicleIMU/LandDetector path. */
|
||||
/* PX4's 0.99/0.01 filter at a 250 Hz reference rate, represented as a
|
||||
* physical time constant so 52/104/208 Hz sensors behave consistently. */
|
||||
config->vibration_filter_time_constant_s =
|
||||
-0.004 / log(0.99);
|
||||
config->accel_vibration_limit_mps2 = 1.2;
|
||||
config->gyro_vibration_limit_rps = 0.02;
|
||||
config->rest_dwell_s = 1.0;
|
||||
}
|
||||
|
||||
static bool config_valid(const fusion_rest_detector_config_t* config)
|
||||
{
|
||||
return (config != NULL) &&
|
||||
isfinite(config->vibration_filter_time_constant_s) &&
|
||||
(config->vibration_filter_time_constant_s > 0.0) &&
|
||||
isfinite(config->accel_vibration_limit_mps2) &&
|
||||
(config->accel_vibration_limit_mps2 > 0.0) &&
|
||||
isfinite(config->gyro_vibration_limit_rps) &&
|
||||
(config->gyro_vibration_limit_rps > 0.0) &&
|
||||
isfinite(config->rest_dwell_s) && (config->rest_dwell_s > 0.0);
|
||||
}
|
||||
|
||||
bool fusion_rest_detector_init(
|
||||
fusion_rest_detector_t* detector,
|
||||
const fusion_rest_detector_config_t* config,
|
||||
uint64_t timebase_hz,
|
||||
bool landed)
|
||||
{
|
||||
if((detector == NULL) || !config_valid(config) || (timebase_hz == 0u))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
memset(detector, 0, sizeof(*detector));
|
||||
detector->config = *config;
|
||||
detector->timebase_hz = timebase_hz;
|
||||
detector->landed = landed;
|
||||
return true;
|
||||
}
|
||||
|
||||
void fusion_rest_detector_set_landed(
|
||||
fusion_rest_detector_t* detector,
|
||||
bool landed,
|
||||
uint64_t timestamp_ticks)
|
||||
{
|
||||
if(detector == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
detector->landed = landed;
|
||||
if(!landed)
|
||||
{
|
||||
detector->at_rest = false;
|
||||
detector->last_motion_ticks = timestamp_ticks;
|
||||
}
|
||||
}
|
||||
|
||||
bool fusion_rest_detector_update(
|
||||
fusion_rest_detector_t* detector,
|
||||
const imu_sample_t* sample)
|
||||
{
|
||||
if((detector == NULL) || (sample == NULL) ||
|
||||
!isfinite(sample->accel_mps2[0]) ||
|
||||
!isfinite(sample->accel_mps2[1]) ||
|
||||
!isfinite(sample->accel_mps2[2]) ||
|
||||
!isfinite(sample->gyro_rps[0]) ||
|
||||
!isfinite(sample->gyro_rps[1]) ||
|
||||
!isfinite(sample->gyro_rps[2]) ||
|
||||
(detector->have_previous_sample &&
|
||||
(sample->timestamp_ticks <= detector->last_sample_ticks)))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if(!detector->have_previous_sample)
|
||||
{
|
||||
memcpy(detector->previous_accel_mps2, sample->accel_mps2,
|
||||
sizeof(detector->previous_accel_mps2));
|
||||
memcpy(detector->previous_gyro_rps, sample->gyro_rps,
|
||||
sizeof(detector->previous_gyro_rps));
|
||||
detector->last_sample_ticks = sample->timestamp_ticks;
|
||||
detector->last_motion_ticks = sample->timestamp_ticks;
|
||||
detector->have_previous_sample = true;
|
||||
detector->at_rest = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
double accel_difference = difference_norm3(
|
||||
sample->accel_mps2, detector->previous_accel_mps2);
|
||||
double gyro_difference = difference_norm3(
|
||||
sample->gyro_rps, detector->previous_gyro_rps);
|
||||
double dt_s = (double)(sample->timestamp_ticks - detector->last_sample_ticks) /
|
||||
(double)detector->timebase_hz;
|
||||
double alpha = 1.0 - exp(
|
||||
-dt_s / detector->config.vibration_filter_time_constant_s);
|
||||
detector->accel_vibration_metric_mps2 =
|
||||
(1.0 - alpha) * detector->accel_vibration_metric_mps2 +
|
||||
alpha * accel_difference;
|
||||
detector->gyro_vibration_metric_rps =
|
||||
(1.0 - alpha) * detector->gyro_vibration_metric_rps +
|
||||
alpha * gyro_difference;
|
||||
memcpy(detector->previous_accel_mps2, sample->accel_mps2,
|
||||
sizeof(detector->previous_accel_mps2));
|
||||
memcpy(detector->previous_gyro_rps, sample->gyro_rps,
|
||||
sizeof(detector->previous_gyro_rps));
|
||||
detector->last_sample_ticks = sample->timestamp_ticks;
|
||||
|
||||
if((detector->accel_vibration_metric_mps2 >
|
||||
detector->config.accel_vibration_limit_mps2) ||
|
||||
(detector->gyro_vibration_metric_rps >
|
||||
detector->config.gyro_vibration_limit_rps))
|
||||
{
|
||||
detector->last_motion_ticks = sample->timestamp_ticks;
|
||||
}
|
||||
|
||||
uint64_t dwell_ticks = (uint64_t)(
|
||||
detector->config.rest_dwell_s * (double)detector->timebase_hz + 0.5);
|
||||
detector->at_rest = detector->landed &&
|
||||
((sample->timestamp_ticks - detector->last_motion_ticks) >= dwell_ticks);
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,162 @@
|
||||
#include "fusion/navigation_fusion.h"
|
||||
|
||||
#include "fusion/fusion_gps_adapter.h"
|
||||
#include "fusion/fusion_rest_detector.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
static fusion_gps_adapter_t gps_adapter;
|
||||
static fusion_engine_t fusion_engine;
|
||||
static fusion_rest_detector_t rest_detector;
|
||||
static bool fusion_initialized;
|
||||
static uint64_t imu_processed_count;
|
||||
|
||||
bool navigation_fusion_init(
|
||||
uint64_t local_timebase_hz,
|
||||
const fusion_imu_quality_profile_t* imu_quality,
|
||||
bool platform_landed)
|
||||
{
|
||||
fusion_engine_config_t engine_config;
|
||||
fusion_gps_adapter_config_t adapter_config;
|
||||
fusion_rest_detector_config_t rest_config;
|
||||
if((local_timebase_hz == 0u) ||
|
||||
!fusion_imu_quality_is_valid(imu_quality))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
fusion_engine_default_config(&engine_config, local_timebase_hz);
|
||||
if(!fusion_engine_apply_imu_quality(&engine_config, imu_quality))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
fusion_gps_adapter_default_config(&adapter_config);
|
||||
fusion_rest_detector_default_config(&rest_config);
|
||||
if(!fusion_engine_init(&fusion_engine, &engine_config) ||
|
||||
!fusion_gps_adapter_init(&gps_adapter, &adapter_config) ||
|
||||
!fusion_rest_detector_init(
|
||||
&rest_detector, &rest_config, local_timebase_hz, platform_landed))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
imu_processed_count = 0u;
|
||||
fusion_initialized = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
void navigation_fusion_set_platform_landed(
|
||||
bool platform_landed,
|
||||
uint64_t timestamp_ticks)
|
||||
{
|
||||
if(fusion_initialized)
|
||||
{
|
||||
fusion_rest_detector_set_landed(
|
||||
&rest_detector, platform_landed, timestamp_ticks);
|
||||
}
|
||||
}
|
||||
|
||||
bool navigation_fusion_push_imu(const imu_sample_t* measurement)
|
||||
{
|
||||
if(!fusion_initialized || (measurement == NULL))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(!fusion_rest_detector_update(&rest_detector, measurement))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
fusion_imu_measurement_t fusion_measurement;
|
||||
memset(&fusion_measurement, 0, sizeof(fusion_measurement));
|
||||
fusion_measurement.timestamp_ticks = measurement->timestamp_ticks;
|
||||
memcpy(fusion_measurement.accel_mps2, measurement->accel_mps2,
|
||||
sizeof(fusion_measurement.accel_mps2));
|
||||
memcpy(fusion_measurement.gyro_rps, measurement->gyro_rps,
|
||||
sizeof(fusion_measurement.gyro_rps));
|
||||
fusion_measurement.temperature_c = measurement->temperature_c;
|
||||
fusion_measurement.sequence = measurement->sequence;
|
||||
fusion_measurement.vehicle_at_rest = rest_detector.at_rest;
|
||||
if(!fusion_engine_push_imu(&fusion_engine, &fusion_measurement))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
imu_processed_count++;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement)
|
||||
{
|
||||
if(!fusion_initialized)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
fusion_position_measurement_t position;
|
||||
if(!fusion_gps_adapter_convert(&gps_adapter, measurement, &position))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return fusion_engine_push_position(&fusion_engine, &position, NULL, NULL);
|
||||
}
|
||||
|
||||
bool navigation_fusion_get_output(navigation_fusion_output_t* output)
|
||||
{
|
||||
if((output == NULL) || !fusion_initialized)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
memset(output, 0, sizeof(*output));
|
||||
output->engine_state = fusion_engine.state;
|
||||
output->calibration_sample_count = fusion_engine.calibration.sample_count;
|
||||
output->gps_accepted = fusion_engine.diagnostics.position_accepted;
|
||||
output->gps_nis_rejected = fusion_engine.diagnostics.position_nis_rejected;
|
||||
output->gps_position_recoveries =
|
||||
fusion_engine.diagnostics.position_recovery_count;
|
||||
output->zero_update_count =
|
||||
fusion_engine.diagnostics.zero_velocity_update_count;
|
||||
output->replay_count = fusion_engine.diagnostics.replay_count;
|
||||
output->stale_gps_count =
|
||||
fusion_engine.diagnostics.position_stale_rejected;
|
||||
output->imu_processed_count = imu_processed_count;
|
||||
if(fusion_engine.state != FUSION_ENGINE_RUNNING)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
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->vehicle_at_rest = runtime->vehicle_at_rest;
|
||||
output->local_valid = true;
|
||||
if(!fusion_gps_adapter_local_to_geodetic(
|
||||
&gps_adapter, runtime->filter.position_m,
|
||||
&output->latitude_deg, &output->longitude_deg, &output->height_m))
|
||||
{
|
||||
/* IMU propagation is still a valid local dead-reckoning state. Without
|
||||
* an externally established origin it must not be labelled as LLH. */
|
||||
return true;
|
||||
}
|
||||
output->global_valid = true;
|
||||
output->valid = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
const char* navigation_fusion_state_name(fusion_engine_state_t state)
|
||||
{
|
||||
switch(state)
|
||||
{
|
||||
case FUSION_ENGINE_CALIBRATING: return "calibrating";
|
||||
case FUSION_ENGINE_RUNNING: return "running";
|
||||
case FUSION_ENGINE_FAULT: return "fault";
|
||||
default: return "unknown";
|
||||
}
|
||||
}
|
||||
+29
-14
@@ -1,11 +1,11 @@
|
||||
#include <stdlib.h>
|
||||
#include "NavEquations.h"
|
||||
#include "Navigation.h"
|
||||
#include "gps_ip_defines.h"
|
||||
#include "gps.h"
|
||||
#include "gps/NavEquations.h"
|
||||
#include "gps/Navigation.h"
|
||||
#include "gps/gps_ip_defines.h"
|
||||
#include "gps/gps.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include "defines.h"
|
||||
#include "gps/defines.h"
|
||||
|
||||
|
||||
/* Function: double computeRange(range_t* rho, ephem_t eph, ionoutc_t* ionoutc, gpstime_t g, double xyz[], t_temp_pos* tempPos)
|
||||
@@ -1199,7 +1199,13 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
|
||||
solution_out->residual_test_valid = residual_test_valid;
|
||||
solution_out->chi_square_pass = chi_square_pass;
|
||||
solution_out->fde_applied = false;
|
||||
solution_out->solution_valid = chi_square_pass;
|
||||
/* Four observations exactly determine x/y/z plus receiver clock bias. With
|
||||
* zero residual DOF no chi-square integrity statement is possible, but the
|
||||
* finite WLS solution and geometry covariance are still a valid minimum fix. */
|
||||
solution_out->solution_valid = solution_out->candidate_valid &&
|
||||
(((solution_out->used_satellite_count == 4u) &&
|
||||
(solution_out->degrees_of_freedom == 0u)) ||
|
||||
(solution_out->residual_test_valid && solution_out->chi_square_pass));
|
||||
solution_out->iteration_count = iteration_count;
|
||||
solution_out->candidate_cycles = gps_read_cycle_counter() - candidate_start_cycles;
|
||||
|
||||
@@ -1386,8 +1392,10 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
|
||||
? (gps_read_cycle_counter() - fde_start_cycles) : 0u;
|
||||
selected_solution.total_wls_cycles = gps_read_cycle_counter() - total_wls_start_cycles;
|
||||
selected_solution.solution_valid = selected_solution.candidate_valid &&
|
||||
selected_solution.residual_test_valid &&
|
||||
selected_solution.chi_square_pass;
|
||||
(((selected_solution.used_satellite_count == 4u) &&
|
||||
(selected_solution.degrees_of_freedom == 0u)) ||
|
||||
(selected_solution.residual_test_valid &&
|
||||
selected_solution.chi_square_pass));
|
||||
if(selected_solution.solution_valid)
|
||||
{
|
||||
gps_wls_last_solution[0] = selected_solution.ecef_m[0];
|
||||
@@ -1398,12 +1406,19 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
|
||||
}
|
||||
*solution_out = selected_solution;
|
||||
|
||||
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
|
||||
"GDOP=%.10lf\r\n", solution_out->gdop);
|
||||
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
|
||||
"position lat=%.20lf lon=%.20lf height=%.3lf\r\n",
|
||||
solution_out->latitude_deg, solution_out->longitude_deg,
|
||||
solution_out->height_m);
|
||||
/* Print only publishable fixes. A four-satellite minimum fix is publishable,
|
||||
* although its separate integrity flag remains false because residual DOF=0. */
|
||||
if(solution_out->solution_valid)
|
||||
{
|
||||
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
|
||||
"DOP gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf\r\n",
|
||||
solution_out->gdop, solution_out->pdop, solution_out->hdop,
|
||||
solution_out->vdop, solution_out->tdop);
|
||||
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
|
||||
"position lat=%.20lf lon=%.20lf height=%.3lf\r\n",
|
||||
solution_out->latitude_deg, solution_out->longitude_deg,
|
||||
solution_out->height_m);
|
||||
}
|
||||
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
|
||||
"NAVSOL valid=%u integrity=%u fde=%u sats=%u/%u excluded=0x%03x "
|
||||
"residual_rms=%.3lf max_residual=%.3lf weighted_rms=%.3lf "
|
||||
+28
-8
@@ -1,9 +1,8 @@
|
||||
#include "linked_list.h"
|
||||
#include "gps_interrupt_handlers.h"
|
||||
#include "Navigation.h"
|
||||
#include "gps_pps.h"
|
||||
#include "gps_time_anchor.h"
|
||||
#include "imu_sensor.h"
|
||||
#include "gps/linked_list.h"
|
||||
#include "gps/gps_interrupt_handlers.h"
|
||||
#include "gps/Navigation.h"
|
||||
#include "gps/gps_pps.h"
|
||||
#include "gps/gps_time_anchor.h"
|
||||
#include <bsp/irq.h>
|
||||
#include <bsp/mmio.h>
|
||||
#include <bsp/plic.h>
|
||||
@@ -74,6 +73,8 @@ static volatile bool gps_timer_maintenance_pending = true;
|
||||
static volatile bool gps_nav_solve_period_pending = false;
|
||||
static volatile gps_runtime_diagnostics_t gps_runtime_diagnostics;
|
||||
static unsigned int gps_acq_round_robin_prn = 0;
|
||||
static unsigned int gps_external_irq_id = 0u;
|
||||
static gps_external_irq_handler_t gps_external_irq_handler = NULL;
|
||||
|
||||
#define GPS_PLIC_DRAIN_LIMIT (GPS_TRCK_IP_COUNT + GPS_ACQ_IP_COUNT + 4u)
|
||||
|
||||
@@ -295,6 +296,8 @@ static void gps_runtime_state_init(void)
|
||||
gps_runtime_diagnostics.last_connected_prn = GPS_PRN_INVALID;
|
||||
gps_runtime_diagnostics.last_lost_prn = GPS_PRN_INVALID;
|
||||
gps_acq_round_robin_prn = 0;
|
||||
gps_external_irq_id = 0u;
|
||||
gps_external_irq_handler = NULL;
|
||||
|
||||
for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
|
||||
{
|
||||
@@ -325,6 +328,22 @@ static void gps_runtime_state_init(void)
|
||||
|
||||
}
|
||||
|
||||
bool gps_interrupt_attach_external_source(
|
||||
unsigned int irq_id,
|
||||
gps_external_irq_handler_t handler)
|
||||
{
|
||||
if((irq_id == 0u) || (handler == NULL))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
uint64_t irq_state = bsp_irq_save();
|
||||
gps_external_irq_id = irq_id;
|
||||
gps_external_irq_handler = handler;
|
||||
bsp_irq_restore(irq_state);
|
||||
return true;
|
||||
}
|
||||
|
||||
static void gps_dispatch_tracking(unsigned char trck_ip_no)
|
||||
{
|
||||
trck_intr_handler(
|
||||
@@ -395,9 +414,10 @@ void gps_interrupt_init(void)
|
||||
|
||||
void gps_dispatch_irq(unsigned int irq_id)
|
||||
{
|
||||
if(irq_id == BSP_PMOD_IRQ)
|
||||
if((irq_id == gps_external_irq_id) &&
|
||||
(gps_external_irq_handler != NULL))
|
||||
{
|
||||
imu_sensor_irq_handler();
|
||||
gps_external_irq_handler();
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -3,14 +3,13 @@
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include "NavEquations.h"
|
||||
#include "Navigation.h"
|
||||
#include "gps_ip_defines.h"
|
||||
#include "linked_list.h"
|
||||
#include "gps_interrupt_handlers.h"
|
||||
#include "gps_pps.h"
|
||||
#include "gps_fusion_publisher.h"
|
||||
#include "defines.h"
|
||||
#include "gps/NavEquations.h"
|
||||
#include "gps/Navigation.h"
|
||||
#include "gps/gps_ip_defines.h"
|
||||
#include "gps/linked_list.h"
|
||||
#include "gps/gps_interrupt_handlers.h"
|
||||
#include "gps/gps_pps.h"
|
||||
#include "gps/defines.h"
|
||||
#include <bsp/irq.h>
|
||||
|
||||
#define GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_SAMPLES ((uint32_t)(SAMPLES_PER_CODE * (double)GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS))
|
||||
@@ -1086,11 +1085,19 @@ static void gps_log_nav_jump_diagnostic(const gps_nav_solution_t* solution,
|
||||
#endif
|
||||
|
||||
#ifdef EKF_ENABLE
|
||||
void calculate_position(Ekf* g_ekf)
|
||||
bool calculate_position(Ekf* g_ekf, gps_nav_solution_t* solution_out,
|
||||
uint64_t* measurement_timestamp_ticks_out)
|
||||
#else
|
||||
void calculate_position()
|
||||
bool calculate_position(gps_nav_solution_t* solution_out,
|
||||
uint64_t* measurement_timestamp_ticks_out)
|
||||
#endif
|
||||
{
|
||||
if((solution_out == NULL) || (measurement_timestamp_ticks_out == NULL))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
*measurement_timestamp_ticks_out = 0u;
|
||||
bool result_ready = false;
|
||||
gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT];
|
||||
unsigned char snapshot_ips[GPS_TRCK_IP_COUNT];
|
||||
uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT];
|
||||
@@ -1186,13 +1193,14 @@ void calculate_position()
|
||||
uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles;
|
||||
if(nav_ok)
|
||||
{
|
||||
nav_solution.tracked_satellite_count = busy_count;
|
||||
uint64_t measurement_timestamp = gps_nav_median_local_timestamp(
|
||||
snapshot_local_timestamps, satellite_count);
|
||||
if(!gps_fusion_publish_solution(&nav_solution, measurement_timestamp))
|
||||
if(measurement_timestamp != 0u)
|
||||
{
|
||||
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
|
||||
"fusion GPS publish skipped timestamp=%lu\r\n",
|
||||
(unsigned long)measurement_timestamp);
|
||||
*solution_out = nav_solution;
|
||||
*measurement_timestamp_ticks_out = measurement_timestamp;
|
||||
result_ready = true;
|
||||
}
|
||||
uint64_t consume_irq_state = bsp_irq_save();
|
||||
gps_consume_nav_sequences_locked(snapshot_ips, snapshot_sequences, satellite_count);
|
||||
@@ -1264,6 +1272,7 @@ void calculate_position()
|
||||
}
|
||||
}
|
||||
}
|
||||
return result_ready;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#include "gps_pps.h"
|
||||
#include "gps/gps_pps.h"
|
||||
|
||||
#include <bsp/soc.h>
|
||||
#include <math.h>
|
||||
@@ -0,0 +1,44 @@
|
||||
#include "gps/gps_receiver.h"
|
||||
|
||||
#include "gps/NavEquations.h"
|
||||
#include "gps/gps_interrupt_handlers.h"
|
||||
#include "gps/gps_ip_defines.h"
|
||||
|
||||
void gps_receiver_initialize(unsigned int nav_solve_timer_ticks)
|
||||
{
|
||||
gps_startup_initialization(nav_solve_timer_ticks);
|
||||
}
|
||||
|
||||
bool gps_receiver_attach_external_irq(
|
||||
unsigned int irq_id,
|
||||
gps_receiver_external_irq_handler_t handler)
|
||||
{
|
||||
return gps_interrupt_attach_external_source(irq_id, handler);
|
||||
}
|
||||
|
||||
void gps_receiver_start_acquisition(void)
|
||||
{
|
||||
acq_start_loop();
|
||||
}
|
||||
|
||||
void gps_receiver_process_pending_events(void)
|
||||
{
|
||||
gps_background_task();
|
||||
}
|
||||
|
||||
bool gps_receiver_solve_ready_epoch(
|
||||
gps_nav_solution_t* solution_out,
|
||||
uint64_t* measurement_timestamp_ticks_out)
|
||||
{
|
||||
return calculate_position(solution_out, measurement_timestamp_ticks_out);
|
||||
}
|
||||
|
||||
uint64_t gps_receiver_status_ticks(void)
|
||||
{
|
||||
return gps_get_systick();
|
||||
}
|
||||
|
||||
uint64_t gps_receiver_status_ticks_per_second(void)
|
||||
{
|
||||
return GPS_SYSTICK_HZ;
|
||||
}
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
#include "gps_time_anchor.h"
|
||||
#include "gps/gps_time_anchor.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
#include "linked_list.h"
|
||||
#include "gps/linked_list.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
+2
-2
@@ -5,8 +5,8 @@
|
||||
* (c) Copyright by ANKASYS
|
||||
**/
|
||||
|
||||
#include "matrixLib.h"
|
||||
#include "gps_macros.h"
|
||||
#include "gps/matrixLib.h"
|
||||
#include "gps/gps_macros.h"
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
|
||||
@@ -1,30 +0,0 @@
|
||||
#include "gps_fusion_publisher.h"
|
||||
|
||||
#include "navigation_fusion.h"
|
||||
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
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);
|
||||
}
|
||||
+2
-2
@@ -1,4 +1,4 @@
|
||||
#include "imu_calibration.h"
|
||||
#include "imu/imu_calibration.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
@@ -60,7 +60,7 @@ bool imu_calibration_is_valid(const imu_calibration_t* calibration)
|
||||
}
|
||||
|
||||
bool imu_calibration_apply(const imu_calibration_t* calibration,
|
||||
fusion_imu_measurement_t* measurement)
|
||||
imu_sample_t* measurement)
|
||||
{
|
||||
if(!imu_calibration_is_valid(calibration) || (measurement == NULL) ||
|
||||
!vector_is_finite(measurement->accel_mps2) ||
|
||||
+69
-14
@@ -1,14 +1,14 @@
|
||||
#include "imu_sensor.h"
|
||||
#include "imu/imu_sensor.h"
|
||||
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
|
||||
#include "tdk_icm42688.h"
|
||||
#include "drivers/imu/tdk_icm42688.h"
|
||||
#endif
|
||||
|
||||
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
|
||||
#include "st_lsm6dsox.h"
|
||||
#include "drivers/imu/st_lsm6dsox.h"
|
||||
#endif
|
||||
|
||||
#if !defined(IMU_SENSOR_BUILD_AUTO) && !defined(IMU_SENSOR_BUILD_TDK) && \
|
||||
@@ -18,9 +18,47 @@
|
||||
|
||||
static imu_sensor_backend_t active_backend;
|
||||
static imu_calibration_t active_calibration;
|
||||
static uint32_t requested_odr_hz;
|
||||
static uint32_t active_odr_hz;
|
||||
static uint64_t calibration_rejected_count;
|
||||
|
||||
#define IMU_ODR_MAX_ERROR_PERCENT 5u
|
||||
|
||||
static bool nearest_supported_odr(uint32_t requested,
|
||||
const uint32_t* supported,
|
||||
unsigned int count,
|
||||
uint32_t* selected)
|
||||
{
|
||||
if((supported == NULL) || (count == 0u) || (selected == NULL))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(requested == 0u)
|
||||
{
|
||||
*selected = 0u;
|
||||
return true;
|
||||
}
|
||||
uint32_t best = supported[0];
|
||||
uint32_t best_error = (best > requested) ? best - requested : requested - best;
|
||||
for(unsigned int i = 1u ; i < count ; i++)
|
||||
{
|
||||
uint32_t error = (supported[i] > requested)
|
||||
? supported[i] - requested : requested - supported[i];
|
||||
if(error < best_error)
|
||||
{
|
||||
best = supported[i];
|
||||
best_error = error;
|
||||
}
|
||||
}
|
||||
if(((uint64_t)best_error * 100u) >
|
||||
((uint64_t)requested * IMU_ODR_MAX_ERROR_PERCENT))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
*selected = best;
|
||||
return true;
|
||||
}
|
||||
|
||||
void imu_sensor_default_config(imu_sensor_config_t* config)
|
||||
{
|
||||
if(config == NULL)
|
||||
@@ -72,6 +110,9 @@ static bool tdk_gyro_range(unsigned int full_scale_dps,
|
||||
|
||||
static bool try_tdk(const imu_sensor_config_t* config)
|
||||
{
|
||||
static const uint32_t supported_odr_hz[] = {
|
||||
25u, 50u, 100u, 200u, 500u, 1000u
|
||||
};
|
||||
if(!imu_calibration_is_valid(
|
||||
&config->calibration_by_backend[IMU_SENSOR_TDK_ICM42688]))
|
||||
{
|
||||
@@ -79,9 +120,17 @@ static bool try_tdk(const imu_sensor_config_t* config)
|
||||
}
|
||||
tdk_icm42688_config_t backend_config;
|
||||
tdk_icm42688_default_config(&backend_config);
|
||||
if(config->odr_hz != 0u)
|
||||
uint32_t selected_odr_hz;
|
||||
if(!nearest_supported_odr(config->odr_hz, supported_odr_hz,
|
||||
sizeof(supported_odr_hz) /
|
||||
sizeof(supported_odr_hz[0]),
|
||||
&selected_odr_hz))
|
||||
{
|
||||
backend_config.odr_hz = config->odr_hz;
|
||||
return false;
|
||||
}
|
||||
if(selected_odr_hz != 0u)
|
||||
{
|
||||
backend_config.odr_hz = selected_odr_hz;
|
||||
}
|
||||
if((config->accel_full_scale_g != 0u) &&
|
||||
!tdk_accel_range(config->accel_full_scale_g,
|
||||
@@ -110,6 +159,7 @@ static bool try_tdk(const imu_sensor_config_t* config)
|
||||
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
|
||||
static bool try_st(const imu_sensor_config_t* config)
|
||||
{
|
||||
static const uint32_t supported_odr_hz[] = {52u, 104u, 208u};
|
||||
if(!imu_calibration_is_valid(
|
||||
&config->calibration_by_backend[IMU_SENSOR_ST_LSM6DSOX]))
|
||||
{
|
||||
@@ -117,9 +167,17 @@ static bool try_st(const imu_sensor_config_t* config)
|
||||
}
|
||||
st_lsm6dsox_config_t backend_config;
|
||||
st_lsm6dsox_default_config(&backend_config);
|
||||
if(config->odr_hz != 0u)
|
||||
uint32_t selected_odr_hz;
|
||||
if(!nearest_supported_odr(config->odr_hz, supported_odr_hz,
|
||||
sizeof(supported_odr_hz) /
|
||||
sizeof(supported_odr_hz[0]),
|
||||
&selected_odr_hz))
|
||||
{
|
||||
backend_config.odr_hz = config->odr_hz;
|
||||
return false;
|
||||
}
|
||||
if(selected_odr_hz != 0u)
|
||||
{
|
||||
backend_config.odr_hz = selected_odr_hz;
|
||||
}
|
||||
if(config->accel_full_scale_g != 0u)
|
||||
{
|
||||
@@ -144,12 +202,14 @@ static bool try_st(const imu_sensor_config_t* config)
|
||||
bool imu_sensor_init(const imu_sensor_config_t* config)
|
||||
{
|
||||
active_backend = IMU_SENSOR_NONE;
|
||||
requested_odr_hz = 0u;
|
||||
active_odr_hz = 0u;
|
||||
calibration_rejected_count = 0u;
|
||||
if(config == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
requested_odr_hz = config->odr_hz;
|
||||
|
||||
#if defined(IMU_SENSOR_BUILD_TDK)
|
||||
return try_tdk(config);
|
||||
@@ -232,7 +292,7 @@ bool imu_sensor_requires_foreground_service(void)
|
||||
return false;
|
||||
}
|
||||
|
||||
bool imu_sensor_take_measurement(fusion_imu_measurement_t* measurement_out)
|
||||
bool imu_sensor_take_measurement(imu_sample_t* measurement_out)
|
||||
{
|
||||
if(measurement_out == NULL)
|
||||
{
|
||||
@@ -276,6 +336,7 @@ void imu_sensor_get_diagnostics(imu_sensor_diagnostics_t* diagnostics_out)
|
||||
}
|
||||
memset(diagnostics_out, 0, sizeof(*diagnostics_out));
|
||||
diagnostics_out->backend = active_backend;
|
||||
diagnostics_out->requested_odr_hz = requested_odr_hz;
|
||||
diagnostics_out->configured_odr_hz = active_odr_hz;
|
||||
diagnostics_out->foreground_polling =
|
||||
imu_sensor_requires_foreground_service();
|
||||
@@ -333,12 +394,6 @@ imu_sensor_backend_t imu_sensor_active_backend(void)
|
||||
return active_backend;
|
||||
}
|
||||
|
||||
bool imu_sensor_active_quality_profile(
|
||||
fusion_imu_quality_profile_t* profile_out)
|
||||
{
|
||||
return imu_sensor_quality_profile_for_backend(active_backend, profile_out);
|
||||
}
|
||||
|
||||
const char* imu_sensor_backend_name(imu_sensor_backend_t backend)
|
||||
{
|
||||
switch(backend)
|
||||
@@ -1,115 +0,0 @@
|
||||
#include "navigation_fusion.h"
|
||||
|
||||
#include "fusion_bus.h"
|
||||
#include "fusion_gps_adapter.h"
|
||||
#include "imu_sensor.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
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_imu_quality_profile_t imu_profile;
|
||||
if(imu_sensor_active_quality_profile(&imu_profile) &&
|
||||
!fusion_engine_apply_imu_quality(&engine_config, &imu_profile))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
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;
|
||||
imu_sensor_service();
|
||||
fusion_imu_measurement_t imu;
|
||||
while(imu_sensor_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;
|
||||
}
|
||||
@@ -8,7 +8,7 @@ TARGET := test_imu_calibration
|
||||
|
||||
all: $(TARGET)
|
||||
|
||||
$(TARGET): test_imu_calibration.c $(ROOT)/src/imu_calibration.c
|
||||
$(TARGET): test_imu_calibration.c $(ROOT)/src/imu/imu_calibration.c
|
||||
$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
run: $(TARGET)
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#include "imu_calibration.h"
|
||||
#include "imu/imu_calibration.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
@@ -14,12 +14,12 @@ static void test_identity_is_noop(void)
|
||||
{
|
||||
imu_calibration_t calibration;
|
||||
imu_calibration_default(&calibration);
|
||||
fusion_imu_measurement_t measurement = {
|
||||
imu_sample_t measurement = {
|
||||
.accel_mps2 = {1.0, -2.0, 9.7},
|
||||
.gyro_rps = {0.01, -0.02, 0.03},
|
||||
.temperature_c = 41.0
|
||||
};
|
||||
fusion_imu_measurement_t expected = measurement;
|
||||
imu_sample_t expected = measurement;
|
||||
assert(imu_calibration_apply(&calibration, &measurement));
|
||||
assert(memcmp(measurement.accel_mps2, expected.accel_mps2,
|
||||
sizeof(measurement.accel_mps2)) == 0);
|
||||
@@ -46,7 +46,7 @@ static void test_bias_transform_and_temperature(void)
|
||||
calibration.reference_temperature_c = 25.0;
|
||||
calibration.temperature_compensation_enabled = true;
|
||||
|
||||
fusion_imu_measurement_t measurement = {
|
||||
imu_sample_t measurement = {
|
||||
.accel_mps2 = {3.0, 5.0, 7.0},
|
||||
.gyro_rps = {0.4, 0.5, 0.6},
|
||||
.temperature_c = 35.0
|
||||
|
||||
@@ -36,6 +36,7 @@ elif [[ -n "${RISCV_TOOLCHAIN:-}" ]]; then
|
||||
else
|
||||
_gps_candidates=(
|
||||
"$GPS_ROOT/tools/local/riscv/bin"
|
||||
/tools/toolchains-medany/bin
|
||||
/tools/toolchain/bin
|
||||
/tools/toolchains/bin
|
||||
/tools/toolchains/rvtc-medany/bin
|
||||
|
||||
@@ -15,6 +15,7 @@ MILL ?= $(firstword $(wildcard $(SOC_TOOLS_ROOT)/mill-current) $(wildcard $(PROJ
|
||||
FIRTOOL ?= $(firstword $(wildcard $(SOC_TOOLS_ROOT)/circt-current/bin/firtool) $(wildcard $(PROJECT_TOOLS_DIR)/circt/bin/firtool))
|
||||
Z3_LIB_DIR ?= $(firstword $(wildcard $(SOC_TOOLS_ROOT)/z3-current/bin) $(wildcard $(PROJECT_TOOLS_DIR)/z3/*/bin))
|
||||
CIRCT_LIB_DIR ?= $(patsubst %/bin/firtool,%/lib,$(FIRTOOL))
|
||||
VIVADO ?= $(lastword $(sort $(wildcard /tools/Xilinx/*/Vivado/bin/vivado) $(shell command -v vivado 2>/dev/null)))
|
||||
|
||||
# RISCV_TOOLCHAIN may point either to the toolchain root or its bin directory.
|
||||
# TOOLCHAIN keeps compatibility with the existing software Makefiles.
|
||||
@@ -24,6 +25,7 @@ _RISCV_SEARCH_DIRS := \
|
||||
$(RISCV_TOOLCHAIN) \
|
||||
$(RISCV_TOOLCHAIN)/bin \
|
||||
$(PROJECT_TOOLS_DIR)/riscv/bin \
|
||||
/tools/toolchains-medany/bin \
|
||||
/tools/toolchain/bin \
|
||||
/tools/toolchains/bin \
|
||||
/tools/toolchains/rvtc-medany/bin
|
||||
@@ -37,6 +39,15 @@ TOOLCHAIN ?= $(patsubst %/,%,$(dir $(_RISCV_GCC)))
|
||||
RISCV_PREFIX ?= $(if $(_RISCV_GCC),$(patsubst %-gcc,%-,$(notdir $(_RISCV_GCC))),riscv64-unknown-elf-)
|
||||
CROSS ?= $(if $(strip $(TOOLCHAIN)),$(TOOLCHAIN)/)$(RISCV_PREFIX)
|
||||
|
||||
# Some compiler-only toolchains (notably rvtc-medany) do not ship GDB. Keep
|
||||
# using the selected compiler toolchain, but fall back to the full local
|
||||
# toolchain for FPGA debug targets.
|
||||
_RISCV_GDB := $(firstword \
|
||||
$(wildcard $(TOOLCHAIN)/$(RISCV_PREFIX)gdb) \
|
||||
$(wildcard /tools/toolchains/tc/toolchain/bin/$(RISCV_PREFIX)gdb) \
|
||||
$(shell command -v $(RISCV_PREFIX)gdb 2>/dev/null))
|
||||
GDB ?= $(if $(_RISCV_GDB),$(_RISCV_GDB),$(CROSS)gdb)
|
||||
|
||||
# Mill's launcher resolves java through PATH, and build.sc resolves firtool the
|
||||
# same way. CIRCT has an rpath for its own libraries; Z3 is shipped separately.
|
||||
export JAVA_HOME
|
||||
|
||||
Reference in New Issue
Block a user