diff --git a/hardware/external/gps/rtl/pps/README.md b/hardware/external/gps/rtl/pps/README.md index cf52cdc..4c5132b 100644 --- a/hardware/external/gps/rtl/pps/README.md +++ b/hardware/external/gps/rtl/pps/README.md @@ -24,6 +24,15 @@ Q2.30 LSB. Phase, integrator, and rate diagnostics are signed Q32 counter-tick values. `MAX_SLEW_Q32` is the absolute rate limit in Q32 ticks per 125 MHz clock; the reset value is approximately 20 ppm. +The PI controller is the hardware loop filter: `Kp` supplies proportional +phase correction and `Ki` is the retained frequency/integral state. There is +no additional phase IIR and no derivative term. An extra phase IIR was +modelled and rejected because it duplicated the small navigation pre-filter +and added acquisition/profile-transition delay. Software may change `Kp`, +`Ki`, maximum slew and lock policy while running; the core captures the whole +mailbox coherently at the next commit and preserves the integrator across a +normal profile transition. + Each rising PPS edge is held high for 100 ms (12,500,000 tracking-clock cycles at 125 MHz), making the physical output easy to observe without changing the disciplined edge timing. The width is derived from `CLK_FREQ_HZ` and the shared @@ -46,7 +55,17 @@ The `capture_i` vector is synchronous to `PCLK`. Each pulse stores the Gray-synchronized 125 MHz counter and increments that channel's capture sequence. The two-stage crossing adds a small fixed timestamp pipeline offset; the SoC-level connection must use the exact PCLK event that updates -`ABS_SAMPLE/REM_CODE_PHASE`, and that offset must be measured once on hardware. +`ABS_SAMPLE/REM_CODE_PHASE`. `CAPTURE_COUNTER` initially refers to the integer +RF-sample event. Before writing `EPOCH_COUNTER`, navigation software subtracts + +``` +round((REM_CODE_PHASE / 1.023e6) * (125e6 / RF_HZ)) +``` + +counter ticks, so the counter and corrected WLS observation refer to the same +fractional code boundary. This removes an otherwise variable 0--61 ns term. +The remaining PCLK/Gray pipeline delay is fixed and must be measured once on +hardware. ## APB register map @@ -68,15 +87,29 @@ the SoC-level connection must use the exact PCLK event that updates | `0x44` | `COMMIT` | WO | bit 0 commit, bit 1 coarse-align commit | | `0x48..0x74` | diagnostics | RO | sequence, GPS time, phase, integrator, rate and pulse count | | `0x78` | `DIAG_SNAPSHOT` | WO | bit 0 atomically snapshots all multiword diagnostics | -| `0x7c` | `VERSION` | RO | RTL interface version (`0x00020100`) | +| `0x7c` | `VERSION` | RO | RTL interface version (`0x00020200`) | +| `0x88` | `LOOP_CONFIG` | RW shadow | bits 15:8 acquire ticks, 23:16 release ticks, 31:24 consecutive acquire updates; bits 7:0 reserved | -Coarse alignment does not assert lock or drive the pin. Five consecutive PI -updates with at most eight tracking-clock ticks (64 ns at 125 MHz) phase error -acquire lock; an error above 32 ticks (256 ns) releases it. This hysteresis -allows realistic WLS epoch jitter without edge chatter. Holdover -retains an already acquired lock, while a newly committed bad reference drops -lock immediately. `STATUS.LOCKED`, `STATUS.VALID`, and the physical PPS output -therefore distinguish an aligned timebase from a converged one. +Coarse alignment does not assert lock or drive the pin. The reset policy +requires five consecutive raw PI residuals within eight tracking-clock ticks +(64 ns at 125 MHz); a residual above 32 ticks (256 ns) releases lock. Runtime +profiles may widen the release threshold or require more good updates without +filtering the phase seen by the controller. Holdover retains the internal +frequency/lock state but deasserts `STATUS.VALID`; a newly committed bad +reference drops lock immediately. `STATUS.LOCKED`, `STATUS.VALID`, and the +physical PPS output therefore distinguish a held/aligned timebase from a +currently qualified one. + +Firmware uses a two-second no-reference watchdog to enter holdover but keeps +the Doppler-predicted bias and PI state for bounded same-datum gaps. On +recovery it fills all mailbox shadow words, releases holdover immediately +before the normal commit toggle, and therefore applies the first coherent +post-gap reference (v2.2 rejects normal PI commits that arrive while holdover +is still asserted). The physical PPS already runs from the held rate word and +does not phase-step; `STATUS.VALID` may precede the PI apply only by the +bounded synchronizer/commit-pipeline latency. A datum change or an observation +gap beyond the prediction horizon uses a coarse commit and remains invalid +until lock is reacquired. Diagnostic words do not change until another `DIAG_SNAPSHOT` request completes; software polls `STATUS.DIAG_BUSY` before reading the group. The disciplined @@ -104,19 +137,24 @@ Reading `ACTIVE_SEQUENCE` or `PULSE_COUNT` without this request can return the previous snapshot, including reset zero, even while the core is accepting commits and generating pulses. -Run lint plus both unit-level Questa tests from this directory: +Run lint plus all five Questa tests from this directory: ``` make -C tb check ``` `pps_wrapper_tb` uses the same ordered MMIO writes as `gps_system/src/gps_pps.c` and -injects CPU/APB latency. `pps_core_tb` independently stresses signed PI math, +injects CPU/APB latency; it separately verifies that a held normal commit is +not applied and that the firmware's shadow-first release sequence applies the +first recovery update. `pps_core_tb` independently stresses signed PI math, slew/anti-windup saturation, stale/future epochs, phase saturation, sequence wrap, holdover and output disable. `pps_convergence_tb` supplies an independent GPS truth clock, a +15 ppm oscillator error followed by a +8 ppm temperature-like step, deterministic measurement noise, varying 3--27 cycle software latency, and 5 Hz navigation commits. It checks phase and rate convergence numerically. +`pps_adaptive_tb` changes `Kp`, `Ki`, maximum slew and lock policy while the +loop is live, checks integrator preservation, validates raw-residual lock +acquisition/release, and verifies that holdover suppresses validity. `CLK_FREQ_HZ` is reduced only to shorten simulation; tick/Q-format behavior is unchanged. diff --git a/hardware/external/gps/rtl/pps/pps_core.sv b/hardware/external/gps/rtl/pps/pps_core.sv index fa4f4eb..96d3561 100644 --- a/hardware/external/gps/rtl/pps/pps_core.sv +++ b/hardware/external/gps/rtl/pps/pps_core.sv @@ -16,6 +16,7 @@ module pps_core #( input logic signed [pps_pkg::PPS_COEFF_WIDTH-1:0] kp_q46, input logic signed [pps_pkg::PPS_COEFF_WIDTH-1:0] ki_q46, input logic [pps_pkg::PPS_FRACTION_WIDTH-1:0] max_slew_q32, + input logic [31:0] loop_config, output logic commit_ack_toggle, output logic [pps_pkg::PPS_COUNTER_WIDTH-1:0] raw_counter, output logic pps_out, @@ -50,8 +51,7 @@ module pps_core #( PPS_MILLISECONDS_PER_SECOND - 1) / PPS_MILLISECONDS_PER_SECOND); localparam int unsigned PPS_PULSE_COUNTER_WIDTH = (PPS_PULSE_WIDTH_TICKS <= 1) ? 1 : $clog2(PPS_PULSE_WIDTH_TICKS); - localparam int unsigned PPS_LOCK_COUNTER_WIDTH = - (PPS_LOCK_ACQUIRE_UPDATES <= 1) ? 1 : $clog2(PPS_LOCK_ACQUIRE_UPDATES); + localparam int unsigned PPS_LOCK_COUNTER_WIDTH = 8; // A navigation commit is rare (normally 5 Hz), while the disciplined clock // runs at 125 MHz. Keep the per-clock time accumulator short and process a @@ -101,6 +101,9 @@ module pps_core #( logic [PPS_SEQUENCE_WIDTH-1:0] epoch_sequence_work; logic signed [PPS_COEFF_WIDTH-1:0] kp_q46_work, ki_q46_work; logic [PPS_FRACTION_WIDTH-1:0] max_slew_q32_work; + logic [7:0] lock_acquire_ticks_work; + logic [7:0] lock_release_ticks_work; + logic [7:0] lock_acquire_updates_work; logic signed [PPS_INCREMENT_WIDTH-1:0] reference_increment_work; // Registered arithmetic stages. No multiplier feeds another multiplier, @@ -135,13 +138,15 @@ module pps_core #( logic signed [PPS_PRODUCT_WIDTH-1:0] max_slew_wide; logic signed [PPS_STATE_WIDTH-1:0] max_slew_signed; logic [PPS_STATE_WIDTH-1:0] phase_abs_work; + logic [PPS_STATE_WIDTH-1:0] lock_acquire_phase_q32_work; + logic [PPS_STATE_WIDTH-1:0] lock_release_phase_q32_work; assign raw_counter = raw_counter_p; assign pps_out = pps_out_p; assign aligned = aligned_p; assign tracking = aligned_p && track_enable && !holdover; assign locked = locked_p; - assign pps_valid = enable && aligned_p && locked_p; + assign pps_valid = enable && aligned_p && locked_p && !holdover; assign stale_sequence = stale_sequence_p; assign stale_age = stale_age_p; assign rate_saturated = rate_saturated_p; @@ -213,6 +218,10 @@ module pps_core #( max_slew_signed = $signed({32'd0, max_slew_q32_work}); phase_abs_work = phase_work[PPS_STATE_WIDTH-1] ? $unsigned(-phase_work) : $unsigned(phase_work); + lock_acquire_phase_q32_work = + {24'd0, lock_acquire_ticks_work, 32'd0}; + lock_release_phase_q32_work = + {24'd0, lock_release_ticks_work, 32'd0}; end always_ff @(posedge clk or negedge resetn) begin @@ -250,6 +259,9 @@ module pps_core #( kp_q46_work <= '0; ki_q46_work <= '0; max_slew_q32_work <= '0; + lock_acquire_ticks_work <= '0; + lock_release_ticks_work <= '0; + lock_acquire_updates_work <= '0; reference_increment_work <= '0; fraction_ticks_work <= '0; commit_age_ticks_work <= '0; @@ -310,6 +322,9 @@ module pps_core #( kp_q46_work <= kp_q46; ki_q46_work <= ki_q46; max_slew_q32_work <= max_slew_q32; + lock_acquire_ticks_work <= loop_config[15:8]; + lock_release_ticks_work <= loop_config[23:16]; + lock_acquire_updates_work <= loop_config[31:24]; if (aligned_p) reference_increment_work <= $signed({1'b0, NOMINAL_INCREMENT_Q32}) + $signed(rate_word_q32_p); @@ -445,15 +460,16 @@ module pps_core #( // A wider release threshold keeps small measurement noise from // chattering an already valid physical PPS output. if (phase_overflow_work || - (phase_abs_work > PPS_LOCK_RELEASE_PHASE_Q32)) begin + (phase_abs_work > lock_release_phase_q32_work)) begin locked_p <= 1'b0; lock_good_updates_p <= '0; pps_out_p <= 1'b0; pulse_ticks_remaining_p <= '0; end else if (!locked_p) begin - if (phase_abs_work <= PPS_LOCK_ACQUIRE_PHASE_Q32) begin - if (lock_good_updates_p == - PPS_LOCK_COUNTER_WIDTH'(PPS_LOCK_ACQUIRE_UPDATES - 1)) begin + if (phase_abs_work <= lock_acquire_phase_q32_work) begin + if ((lock_acquire_updates_work <= 8'd1) || + (lock_good_updates_p >= + (lock_acquire_updates_work - 1'b1))) begin locked_p <= 1'b1; lock_good_updates_p <= '0; end else begin diff --git a/hardware/external/gps/rtl/pps/pps_pkg.sv b/hardware/external/gps/rtl/pps/pps_pkg.sv index bac7e06..4780ba1 100644 --- a/hardware/external/gps/rtl/pps/pps_pkg.sv +++ b/hardware/external/gps/rtl/pps/pps_pkg.sv @@ -28,6 +28,12 @@ package pps_pkg; localparam logic [PPS_STATE_WIDTH-1:0] PPS_LOCK_RELEASE_PHASE_Q32 = 64'h0000_0020_0000_0000; // 32 tracking-clock ticks (256 ns at 125 MHz) localparam logic [PPS_FRACTION_WIDTH-1:0] PPS_DEFAULT_MAX_SLEW_Q32 = 32'd85899; + localparam logic [31:0] PPS_DEFAULT_LOOP_CONFIG = { + 8'(PPS_LOCK_ACQUIRE_UPDATES), + PPS_LOCK_RELEASE_PHASE_Q32[39:32], + PPS_LOCK_ACQUIRE_PHASE_Q32[39:32], + 8'd0 // reserved + }; localparam logic [PPS_STATE_WIDTH-1:0] PPS_ONE_TICK_Q32 = 64'h0000_0001_0000_0000; typedef enum logic [31:0] { @@ -64,7 +70,8 @@ package pps_pkg; PPS_DIAG_SNAPSHOT_OFFSET = 32'h0000_0078, PPS_VERSION_REG_OFFSET = 32'h0000_007c, PPS_KP_Q46_HI_OFFSET = 32'h0000_0080, - PPS_KI_Q46_HI_OFFSET = 32'h0000_0084 + PPS_KI_Q46_HI_OFFSET = 32'h0000_0084, + PPS_LOOP_CONFIG_OFFSET = 32'h0000_0088 } pps_wrapper_reg_offsets_e; typedef enum int unsigned { @@ -104,7 +111,7 @@ package pps_pkg; PPS_APB_STATE_FINISH } pps_apb_state_e; - localparam logic [31:0] PPS_VERSION = 32'h0002_0100; + localparam logic [31:0] PPS_VERSION = 32'h0002_0200; function automatic logic [PPS_COUNTER_WIDTH-1:0] pps_gray_to_binary( input logic [PPS_COUNTER_WIDTH-1:0] gray); logic [PPS_COUNTER_WIDTH-1:0] binary; diff --git a/hardware/external/gps/rtl/pps/pps_wrapper.sv b/hardware/external/gps/rtl/pps/pps_wrapper.sv index d4fa1d3..094c16f 100644 --- a/hardware/external/gps/rtl/pps/pps_wrapper.sv +++ b/hardware/external/gps/rtl/pps/pps_wrapper.sv @@ -39,6 +39,7 @@ module pps_wrapper #( logic signed [PPS_COEFF_WIDTH-1:0] kp_q46_p, kp_q46_n; logic signed [PPS_COEFF_WIDTH-1:0] ki_q46_p, ki_q46_n; logic [PPS_FRACTION_WIDTH-1:0] max_slew_q32_p, max_slew_q32_n; + logic [31:0] loop_config_p, loop_config_n; logic commit_coarse_p, commit_coarse_n; logic commit_req_toggle_p, commit_req_toggle_n; logic clear_status_toggle_p, clear_status_toggle_n; @@ -65,7 +66,8 @@ module pps_wrapper #( logic [PPS_SEQUENCE_WIDTH-1:0] active_sequence_pps, pulse_count_pps; logic [PPS_TIME_WIDTH-1:0] active_seconds_pps; logic [PPS_STATE_WIDTH-1:0] active_subsec_pps; - logic signed [PPS_STATE_WIDTH-1:0] phase_error_pps, integrator_pps, rate_word_pps; + logic signed [PPS_STATE_WIDTH-1:0] phase_error_pps; + logic signed [PPS_STATE_WIDTH-1:0] integrator_pps, rate_word_pps; (* ASYNC_REG = "TRUE" *) logic [11:0] status_sync1_p, status_sync2_p; logic [PPS_SEQUENCE_WIDTH-1:0] active_sequence_sync1_p, active_sequence_sync2_p; @@ -83,7 +85,8 @@ module pps_wrapper #( logic [PPS_SEQUENCE_WIDTH-1:0] diag_active_sequence_pps, diag_pulse_count_pps; logic [PPS_TIME_WIDTH-1:0] diag_active_seconds_pps; logic [PPS_STATE_WIDTH-1:0] diag_active_subsec_pps; - logic [PPS_STATE_WIDTH-1:0] diag_phase_error_pps, diag_integrator_pps, diag_rate_word_pps; + logic [PPS_STATE_WIDTH-1:0] diag_phase_error_pps; + logic [PPS_STATE_WIDTH-1:0] diag_integrator_pps, diag_rate_word_pps; logic commit_busy; logic diag_busy; @@ -147,6 +150,7 @@ module pps_wrapper #( kp_q46_p <= '0; ki_q46_p <= '0; max_slew_q32_p <= PPS_DEFAULT_MAX_SLEW_Q32; + loop_config_p <= PPS_DEFAULT_LOOP_CONFIG; commit_coarse_p <= 1'b0; commit_req_toggle_p <= 1'b0; clear_status_toggle_p <= 1'b0; @@ -190,6 +194,7 @@ module pps_wrapper #( kp_q46_p <= kp_q46_n; ki_q46_p <= ki_q46_n; max_slew_q32_p <= max_slew_q32_n; + loop_config_p <= loop_config_n; commit_coarse_p <= commit_coarse_n; commit_req_toggle_p <= commit_req_toggle_n; clear_status_toggle_p <= clear_status_toggle_n; @@ -254,6 +259,7 @@ module pps_wrapper #( kp_q46_n = kp_q46_p; ki_q46_n = ki_q46_p; max_slew_q32_n = max_slew_q32_p; + loop_config_n = loop_config_p; commit_coarse_n = commit_coarse_p; commit_req_toggle_n = commit_req_toggle_p; clear_status_toggle_n = clear_status_toggle_p; @@ -318,6 +324,9 @@ module pps_wrapper #( (LOW_ADDRESS + PPS_MAX_SLEW_Q32_OFFSET): begin if (commit_busy) commit_overrun_n = 1'b1; else max_slew_q32_n = PWDATA; end + (LOW_ADDRESS + PPS_LOOP_CONFIG_OFFSET): begin + if (commit_busy) commit_overrun_n = 1'b1; else loop_config_n = PWDATA; + end (LOW_ADDRESS + PPS_COMMIT_REG_OFFSET): begin if (PWDATA[PPS_COMMIT_VALID_POS]) begin if (!commit_busy) begin @@ -357,6 +366,7 @@ module pps_wrapper #( (LOW_ADDRESS + PPS_KP_Q46_HI_OFFSET), (LOW_ADDRESS + PPS_KI_Q46_HI_OFFSET), (LOW_ADDRESS + PPS_MAX_SLEW_Q32_OFFSET), + (LOW_ADDRESS + PPS_LOOP_CONFIG_OFFSET), (LOW_ADDRESS + PPS_COMMIT_REG_OFFSET), (LOW_ADDRESS + PPS_DIAG_SNAPSHOT_OFFSET): begin end default: PSLVERR = 1'b1; @@ -392,6 +402,7 @@ module pps_wrapper #( (LOW_ADDRESS + PPS_KP_Q46_HI_OFFSET): PRDATA = {{16{kp_q46_p[47]}}, kp_q46_p[47:32]}; (LOW_ADDRESS + PPS_KI_Q46_HI_OFFSET): PRDATA = {{16{ki_q46_p[47]}}, ki_q46_p[47:32]}; (LOW_ADDRESS + PPS_MAX_SLEW_Q32_OFFSET): PRDATA = max_slew_q32_p; + (LOW_ADDRESS + PPS_LOOP_CONFIG_OFFSET): PRDATA = loop_config_p; (LOW_ADDRESS + PPS_ACTIVE_SEQUENCE_OFFSET): PRDATA = active_sequence_sync2_p; (LOW_ADDRESS + PPS_ACTIVE_SECONDS_LO_OFFSET): PRDATA = active_seconds_sync2_p[31:0]; (LOW_ADDRESS + PPS_ACTIVE_SECONDS_HI_OFFSET): PRDATA = active_seconds_sync2_p[63:32]; @@ -421,6 +432,7 @@ module pps_wrapper #( .epoch_counter(epoch_counter_shadow_p), .gps_seconds(gps_seconds_shadow_p), .gps_fraction_q32(gps_fraction_shadow_p), .epoch_sequence(epoch_sequence_shadow_p), .kp_q46(kp_q46_p), .ki_q46(ki_q46_p), .max_slew_q32(max_slew_q32_p), + .loop_config(loop_config_p), .commit_ack_toggle(commit_ack_pps), .raw_counter(raw_counter_pps), .pps_out(pps_out), .aligned(aligned_pps), .tracking(tracking_pps), .locked(locked_pps), .pps_valid(pps_valid), diff --git a/hardware/external/gps/rtl/pps/tb/Makefile b/hardware/external/gps/rtl/pps/tb/Makefile index e368ac4..5444d14 100644 --- a/hardware/external/gps/rtl/pps/tb/Makefile +++ b/hardware/external/gps/rtl/pps/tb/Makefile @@ -14,6 +14,7 @@ test: cd $(QUESTA_RUN_DIR) && PPS_TB_DIR=$(CURDIR) PPS_TB_TOP=pps_wrapper_tb $(QUESTA_VSIM) -c -l wrapper.log -wlf wrapper.wlf -do $(CURDIR)/run.do cd $(QUESTA_RUN_DIR) && PPS_TB_DIR=$(CURDIR) PPS_TB_TOP=pps_core_tb $(QUESTA_VSIM) -c -l core.log -wlf core.wlf -do $(CURDIR)/run.do cd $(QUESTA_RUN_DIR) && PPS_TB_DIR=$(CURDIR) PPS_TB_TOP=pps_convergence_tb $(QUESTA_VSIM) -c -l convergence.log -wlf convergence.wlf -do $(CURDIR)/run.do + cd $(QUESTA_RUN_DIR) && PPS_TB_DIR=$(CURDIR) PPS_TB_TOP=pps_adaptive_tb $(QUESTA_VSIM) -c -l adaptive.log -wlf adaptive.wlf -do $(CURDIR)/run.do cd $(QUESTA_RUN_DIR) && PPS_TB_DIR=$(CURDIR) PPS_TB_TOP=pps_precision_tb $(QUESTA_VSIM) -c -l precision.log -wlf precision.wlf -do $(CURDIR)/run.do clean: diff --git a/hardware/external/gps/rtl/pps/tb/pps_adaptive_tb.sv b/hardware/external/gps/rtl/pps/tb/pps_adaptive_tb.sv new file mode 100644 index 0000000..cda10c4 --- /dev/null +++ b/hardware/external/gps/rtl/pps/tb/pps_adaptive_tb.sv @@ -0,0 +1,191 @@ +`timescale 1ns/1ps + +module pps_adaptive_tb; + import pps_pkg::*; + + localparam int unsigned CLK_FREQ_HZ = 1000; + localparam logic [63:0] TICK_Q32 = 64'h0000_0001_0000_0000; + localparam logic [63:0] SECOND_Q32 = CLK_FREQ_HZ * TICK_Q32; + localparam logic [31:0] ACQUIRE_CONFIG = 32'h0540_0800; + localparam logic [31:0] PRECISION_CONFIG = 32'h0340_0800; + localparam logic signed [47:0] ACQUIRE_KP_Q46 = 48'sh0800_0000_0000; + localparam logic signed [47:0] ACQUIRE_KI_Q46 = 48'sh0400_0000_0000; + localparam logic signed [47:0] PRECISION_KP_Q46 = 48'sh0200_0000_0000; + localparam logic signed [47:0] PRECISION_KI_Q46 = 48'sh0100_0000_0000; + + logic clk = 1'b0; + logic resetn = 1'b0; + logic enable = 1'b1; + logic track_enable = 1'b1; + logic holdover = 1'b0; + logic clear_status_toggle = 1'b0; + logic commit_toggle = 1'b0; + logic commit_coarse = 1'b0; + logic [63:0] epoch_counter = '0; + logic [63:0] gps_seconds = '0; + logic [31:0] gps_fraction_q32 = '0; + logic [31:0] epoch_sequence = '0; + logic signed [PPS_COEFF_WIDTH-1:0] kp_q46 = ACQUIRE_KP_Q46; + logic signed [PPS_COEFF_WIDTH-1:0] ki_q46 = ACQUIRE_KI_Q46; + logic [31:0] max_slew_q32 = 32'h7fff_ffff; + logic [31:0] loop_config = ACQUIRE_CONFIG; + logic commit_ack_toggle; + logic [63:0] raw_counter; + logic pps_out, aligned, tracking, locked, pps_valid; + logic stale_sequence, stale_age, rate_saturated, phase_saturated; + logic [31:0] active_sequence, pulse_count; + logic [63:0] active_seconds, active_subsec_q32; + logic signed [63:0] phase_error_q32; + logic signed [63:0] integrator_q32, rate_word_q32; + int checks = 0; + + always #4 clk = ~clk; + + pps_core #(.CLK_FREQ_HZ(CLK_FREQ_HZ)) dut ( + .clk(clk), .resetn(resetn), .enable(enable), .track_enable(track_enable), + .holdover(holdover), .clear_status_toggle(clear_status_toggle), + .commit_toggle(commit_toggle), .commit_coarse(commit_coarse), + .epoch_counter(epoch_counter), .gps_seconds(gps_seconds), + .gps_fraction_q32(gps_fraction_q32), .epoch_sequence(epoch_sequence), + .kp_q46(kp_q46), .ki_q46(ki_q46), .max_slew_q32(max_slew_q32), + .loop_config(loop_config), + .commit_ack_toggle(commit_ack_toggle), .raw_counter(raw_counter), + .pps_out(pps_out), .aligned(aligned), .tracking(tracking), .locked(locked), + .pps_valid(pps_valid), .stale_sequence(stale_sequence), + .stale_age(stale_age), .rate_saturated(rate_saturated), + .phase_saturated(phase_saturated), .active_sequence(active_sequence), + .active_seconds(active_seconds), .active_subsec_q32(active_subsec_q32), + .phase_error_q32(phase_error_q32), .integrator_q32(integrator_q32), + .rate_word_q32(rate_word_q32), .pulse_count(pulse_count) + ); + + task automatic check_adaptive(input logic condition, input string message); + begin + checks = checks + 1; + if (!condition) + $fatal(1, "ADAPTIVE CHECK %0d FAILED: %s", checks, message); + end + endtask + + task automatic commit_anchor( + input logic [63:0] counter_value, + input logic [63:0] seconds_value, + input logic [31:0] fraction_value, + input logic [31:0] sequence_id, + input logic coarse); + logic expected_ack; + int timeout; + begin + @(negedge clk); + epoch_counter = counter_value; + gps_seconds = seconds_value; + gps_fraction_q32 = fraction_value; + epoch_sequence = sequence_id; + commit_coarse = coarse; + commit_toggle = ~commit_toggle; + expected_ack = commit_toggle; + timeout = 0; + while (commit_ack_toggle != expected_ack) begin + @(posedge clk); + timeout = timeout + 1; + if (timeout > 40) $fatal(1, "adaptive commit timeout"); + end + @(negedge clk); + end + endtask + + task automatic commit_current_offset( + input integer signed offset_ticks, + input logic [31:0] sequence_id); + logic [63:0] counter_value; + logic [63:0] seconds_value; + logic signed [64:0] subsec_value; + logic [31:0] fraction_value; + begin + @(negedge clk); + counter_value = raw_counter; + seconds_value = active_seconds; + subsec_value = $signed({1'b0, active_subsec_q32}) + + (offset_ticks * $signed({1'b0, TICK_Q32})); + if (subsec_value >= $signed({1'b0, SECOND_Q32})) begin + subsec_value = subsec_value - $signed({1'b0, SECOND_Q32}); + seconds_value = seconds_value + 64'd1; + end else if (subsec_value < 0) begin + subsec_value = subsec_value + $signed({1'b0, SECOND_Q32}); + seconds_value = seconds_value - 64'd1; + end + fraction_value = subsec_value / CLK_FREQ_HZ; + commit_anchor( + counter_value, seconds_value, fraction_value, sequence_id, 1'b0); + end + endtask + + initial begin + logic [31:0] sequence_before_holdover; + logic signed [63:0] acquire_rate; + logic signed [63:0] acquire_integrator; + + repeat (5) @(posedge clk); + resetn = 1'b1; + repeat (5) @(posedge clk); + + commit_anchor(raw_counter, 64'd100, 32'd0, 32'd1, 1'b1); + check_adaptive(aligned && !locked && !pps_valid, + "coarse alignment starts invalid and unlocked"); + + // Lock detection uses the unfiltered PI phase residual. Five coherent + // updates inside the programmable eight-tick window are required. + for (int update = 0; update < 4; update = update + 1) begin + commit_current_offset(0, update + 2); + check_adaptive(!locked, "lock cannot assert before configured count"); + end + commit_current_offset(0, 32'd6); + check_adaptive(locked && pps_valid, + "raw coherent residuals acquire lock"); + + // Exercise the acquisition gains, then replace Kp, Ki and lock policy + // while running. The core captures one coherent set per commit and keeps + // the integrator state across a profile transition. + commit_current_offset(1, 32'd7); + acquire_rate = rate_word_q32; + acquire_integrator = integrator_q32; + check_adaptive((acquire_rate > 0) && (acquire_integrator > 0), + "acquisition Kp and Ki both contribute"); + + kp_q46 = PRECISION_KP_Q46; + ki_q46 = PRECISION_KI_Q46; + loop_config = PRECISION_CONFIG; + commit_current_offset(1, 32'd8); + check_adaptive(locked && pps_valid, + "on-the-fly PI profile transition preserves lock"); + check_adaptive(integrator_q32 > acquire_integrator, + "new Ki continues the preserved integrator"); + check_adaptive(rate_word_q32 < acquire_rate, + "lower precision Kp/Ki reduce loop correction"); + check_adaptive((dut.kp_q46_work == PRECISION_KP_Q46) && + (dut.ki_q46_work == PRECISION_KI_Q46), + "commit atomically captures runtime Kp and Ki"); + + sequence_before_holdover = active_sequence; + holdover = 1'b1; + repeat (3) @(posedge clk); + check_adaptive(locked && !tracking && !pps_valid, + "holdover retains lock state but marks PPS invalid"); + commit_current_offset(0, 32'd9); + check_adaptive(active_sequence == sequence_before_holdover, + "holdover rejects control updates"); + holdover = 1'b0; + repeat (3) @(posedge clk); + check_adaptive(pps_valid && tracking, + "leaving holdover restores validity without a phase step"); + + commit_current_offset(200, 32'd10); + check_adaptive(!locked && !pps_valid, + "one raw residual beyond release threshold drops lock"); + + $display("PPS ADAPTIVE TB PASS: %0d checks phase=%0d rate=%0d", + checks, phase_error_q32, rate_word_q32); + $finish; + end + +endmodule diff --git a/hardware/external/gps/rtl/pps/tb/pps_convergence_tb.sv b/hardware/external/gps/rtl/pps/tb/pps_convergence_tb.sv index fb6b8ae..030e7d2 100644 --- a/hardware/external/gps/rtl/pps/tb/pps_convergence_tb.sv +++ b/hardware/external/gps/rtl/pps/tb/pps_convergence_tb.sv @@ -75,6 +75,7 @@ module pps_convergence_tb; .epoch_counter(epoch_counter), .gps_seconds(gps_seconds), .gps_fraction_q32(gps_fraction_q32), .epoch_sequence(epoch_sequence), .kp_q46(kp_q46), .ki_q46(ki_q46), .max_slew_q32(max_slew_q32), + .loop_config(PPS_DEFAULT_LOOP_CONFIG), .commit_ack_toggle(commit_ack_toggle), .raw_counter(raw_counter), .pps_out(pps_out), .aligned(aligned), .tracking(tracking), .locked(locked), .pps_valid(pps_valid), diff --git a/hardware/external/gps/rtl/pps/tb/pps_core_tb.sv b/hardware/external/gps/rtl/pps/tb/pps_core_tb.sv index d317819..eeffd75 100644 --- a/hardware/external/gps/rtl/pps/tb/pps_core_tb.sv +++ b/hardware/external/gps/rtl/pps/tb/pps_core_tb.sv @@ -40,6 +40,7 @@ module pps_core_tb; .epoch_counter(epoch_counter), .gps_seconds(gps_seconds), .gps_fraction_q32(gps_fraction_q32), .epoch_sequence(epoch_sequence), .kp_q46(kp_q46), .ki_q46(ki_q46), .max_slew_q32(max_slew_q32), + .loop_config(PPS_DEFAULT_LOOP_CONFIG), .commit_ack_toggle(commit_ack_toggle), .raw_counter(raw_counter), .pps_out(pps_out), .aligned(aligned), .tracking(tracking), .locked(locked), .pps_valid(pps_valid), diff --git a/hardware/external/gps/rtl/pps/tb/pps_wrapper_tb.sv b/hardware/external/gps/rtl/pps/tb/pps_wrapper_tb.sv index c93fdf2..f27c706 100644 --- a/hardware/external/gps/rtl/pps/tb/pps_wrapper_tb.sv +++ b/hardware/external/gps/rtl/pps/tb/pps_wrapper_tb.sv @@ -203,6 +203,9 @@ module pps_wrapper_tb; read_ok(PPS_VERSION_REG_OFFSET, read_data); check_true(read_data == PPS_VERSION, "version register"); + read_ok(PPS_LOOP_CONFIG_OFFSET, read_data); + check_true(read_data == PPS_DEFAULT_LOOP_CONFIG, + "backward-compatible default loop configuration"); // Stop the destination clock to force a real mailbox-busy interval. A // CPU write to any shadow field during that interval must be rejected; @@ -254,6 +257,10 @@ module pps_wrapper_tb; write_ok(PPS_KI_Q46_LO_OFFSET, 32'h0000_0000); write_ok(PPS_KI_Q46_HI_OFFSET, 32'h0000_0000); write_ok(PPS_MAX_SLEW_Q32_OFFSET, 32'h1000_0000); + write_ok(PPS_LOOP_CONFIG_OFFSET, 32'h0540_0800); + read_ok(PPS_LOOP_CONFIG_OFFSET, read_data); + check_true(read_data == 32'h0540_0800, + "runtime loop configuration readback"); // GPS reference says the capture happened at second 100 + 0.2 s. // Software and APB deliberately consume time before COMMIT. The core must @@ -360,10 +367,34 @@ module pps_wrapper_tb; pps_sw_snapshot_diagnostics(); read_ok(PPS_ACTIVE_SEQUENCE_OFFSET, active_sequence); check_true(active_sequence == 7, "holdover preserves the last accepted control epoch"); + check_true(!pps_valid, "holdover deasserts validity while retaining the disciplined timebase"); - apb_read(12'h088, read_data, read_error); + // Firmware writes every shadow field while held, then releases holdover + // immediately before the commit toggle. This makes the first post-gap + // normal update reach the PI instead of receiving a drop-only ACK. + @(negedge pps_clk); + lock_ref_counter = dut.i_pps_core.raw_counter; + lock_ref_seconds = dut.i_pps_core.active_seconds; + lock_ref_fraction = dut.i_pps_core.active_subsec_q32 / CLK_FREQ_HZ; + write_ok(PPS_EPOCH_COUNTER_LO_OFFSET, lock_ref_counter[31:0]); + write_ok(PPS_EPOCH_COUNTER_HI_OFFSET, lock_ref_counter[63:32]); + write_ok(PPS_GPS_SECONDS_LO_OFFSET, lock_ref_seconds[31:0]); + write_ok(PPS_GPS_SECONDS_HI_OFFSET, lock_ref_seconds[63:32]); + write_ok(PPS_GPS_FRACTION_Q32_OFFSET, lock_ref_fraction); + write_ok(PPS_EPOCH_SEQUENCE_OFFSET, 32'd9); + write_ok(PPS_CONTROL_REG_OFFSET, 32'h0000_0003); // release after shadows + write_ok(PPS_COMMIT_REG_OFFSET, 32'h0000_0001); + wait_commit_idle(); + pps_sw_snapshot_diagnostics(); + read_ok(PPS_ACTIVE_SEQUENCE_OFFSET, active_sequence); + check_true(active_sequence == 9, + "shadow-first holdover release applies first recovery update"); + check_true(pps_valid, + "recovery update preserves lock and restores validity"); + + apb_read(12'h08c, read_data, read_error); check_true(read_error, "unmapped APB register raises PSLVERR"); - apb_write(12'h088, 32'h1, read_error); + apb_write(12'h08c, 32'h1, read_error); check_true(read_error, "unmapped APB write raises PSLVERR"); write_ok(PPS_CAPTURE_SELECT_REG_OFFSET, 0); diff --git a/hardware/external/gps/rtl/pps/tb/run.do b/hardware/external/gps/rtl/pps/tb/run.do index cac2f3e..30ce634 100644 --- a/hardware/external/gps/rtl/pps/tb/run.do +++ b/hardware/external/gps/rtl/pps/tb/run.do @@ -9,6 +9,7 @@ vlog -sv \ [file join $tb_dir pps_wrapper_tb.sv] \ [file join $tb_dir pps_core_tb.sv] \ [file join $tb_dir pps_convergence_tb.sv] \ + [file join $tb_dir pps_adaptive_tb.sv] \ [file join $tb_dir pps_precision_tb.sv] vsim -c -voptargs=+acc work.$::env(PPS_TB_TOP) run -all diff --git a/soc/software/Makefile b/soc/software/Makefile index 5536a1f..af0ba7b 100644 --- a/soc/software/Makefile +++ b/soc/software/Makefile @@ -10,7 +10,7 @@ include $(REPO_DIR)/tools/project.mk APP_FLASH_OFFSET ?= 0x06000000 APP ?= hello_uart GPS_DEBUG ?= 1 -GPS_DEBUG_LEVEL ?= 4 +GPS_DEBUG_LEVEL ?= 1 BSP_UART_CLOCK_HZ ?= 100000000 BSP_UART_BAUDRATE ?= 115200 GPS_SYSTEM_UART_BAUDRATE ?= 460800 diff --git a/soc/software/gps_system/Makefile b/soc/software/gps_system/Makefile index aaa9ab5..a187768 100644 --- a/soc/software/gps_system/Makefile +++ b/soc/software/gps_system/Makefile @@ -5,7 +5,7 @@ TARGET := gps_system # directory previously compiled every GPS_LOG call out and produced a silent # UART image. GPS_DEBUG ?= 1 -GPS_DEBUG_LEVEL ?= 4 +GPS_DEBUG_LEVEL ?= 1 GPS_BUILD_ID ?= $(shell git describe --always --dirty --abbrev=12 2>/dev/null || printf unknown) # The current Genesys2 Rocket/fabric domain that clocks the UART is 50 MHz. @@ -71,6 +71,8 @@ APP_SRCS := \ src/gps/gps_time_anchor.c \ src/gps/gps_nav_epoch.c \ src/gps/gps_pps.c \ + src/gps/gps_pps_quality.c \ + src/gps/gps_pps_time_filter.c \ src/gps/gps_tracking_measurement.c \ src/gps/linked_list.c \ src/gps/NavEquations.c \ @@ -79,6 +81,7 @@ APP_SRCS := \ BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c GPS_NAV_SOLVE_TIMER_TICKS ?= 10 +GPS_PPS_STATUS_LOG_INTERVAL ?= 10 include ../common/app.mk @@ -90,7 +93,8 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \ -DIMU_GYRO_FS_DPS=$(IMU_GYRO_FS_DPS) \ -DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \ -DIMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=$(IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES) \ - -DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS) + -DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS) \ + -DGPS_PPS_STATUS_LOG_INTERVAL=$(GPS_PPS_STATUS_LOG_INTERVAL) LDLIBS += -lm ifeq ($(GPS_DEBUG),1) diff --git a/soc/software/gps_system/README.md b/soc/software/gps_system/README.md index 0de8770..d38e84b 100644 --- a/soc/software/gps_system/README.md +++ b/soc/software/gps_system/README.md @@ -1173,7 +1173,8 @@ Bu timer/scheduler yavasligi degildir. Firmware start ediyor; tamamlanma icin RF darbesi uretir. Bu olay `ABS_SAMPLE` ve `REM_CODE_PHASE` latch'iyle aynidir; subframe-ready PPS timestamp'i olusturmaz. - `leastSquaresSTD()` ayni basarili epoch'un `[x, y, z, b]` sonucunu verir. - Minimum corrected-ABS kanalinin receive-time ankraji su sekilde kurulur: + Ilk cozumde en erken corrected-ABS kanali zaman datum'u secilir; ayni SVID + coherent snapshot'ta kaldigi surece referans korunur. Receive-time ankraji: ```text T_rx = TOW_ref + POSTPONE_TOW + STARTOFFSET/1000 - b/c @@ -1182,12 +1183,108 @@ T_rx = TOW_ref + POSTPONE_TOW + STARTOFFSET/1000 - b/c - `POSTPONE_TOW=0.160 s`, takip eden subframe'dan parity icin beklenen sekiz bitin gecikmesidir ve yazilim tarafinda tam bir kez uygulanir. PPS RTL bu degeri tekrar eklemez. -- Bu GPS zamani, ayni minimum-ABS kanalinin 125 MHz capture counter'i ile atomik +- Bu GPS zamani, ayni kalici referans kanalinin 125 MHz capture counter'i ile atomik APB mailbox uzerinden eslenir. Ilk cozum coarse-align, sonraki cozumler Q2.46 dusuk bant genislikli PI rate discipline yapar. WLS/APB calisma suresi faz hatasi olarak kullanilmaz. +- Tracking capture olayi integer RF sample kenarinda olustugu halde WLS, + `ABS_SAMPLE - REM_CODE_PHASE/1.023e6` alt-sample zamanini kullanir. Yazilim + mailbox `EPOCH_COUNTER` degerinden ayni fractional sample gecikmesini + `round(fraction * 125e6/16.367981e6)` ile cikarir. Boylece WLS zamani ile + hardware epoch'u ayni code boundary'ye referans olur; 125 MHz integer + mailbox yuvarlamasindan en fazla 4 ns kalir. +- HOW/sample sureklilik kontrolu kartin fiziksel `16.367981 MHz` RF frekansini + kullanir. Periyodik nav-solve TOW'u receiver propagation/range-rate + degisiminden uretilmez; sample farki yalniz olay numarasini dogrular ve TOW + bilinen 200 ms transmit-time grid'ine snap edilir. +- Donanimdaki kontrol filtresi PI loop'un kendisidir; ayri bir phase-IIR veya + D terimi yoktur. Yazilim `TDOP/GDOP`, formal clock-bias sigma, kullanilan + uydu sayisi, C/N0, chi-square/FDE ve Doppler velocity residual'ina gore + `acquire`, `precision`, `robust` ve `holdover` profillerini histerezisle + secer. Profil gecisinde `Kp`, `Ki`, `max-slew` ve v2.2 lock esikleri bir + sonraki mailbox commit'inde birlikte yakalanir; normal geciste integrator + korunur. Lock almak icin bes ardisik residual `64 ns` icinde olmalidir; + uretim release esigi tek seyrek WLS/capture sicrama pulse kaybettirmesin + diye `1.024 us` tutulur. Watchdog, quality holdover ve `20 ppm` slew clamp + buyuk arizalari ayri olarak sinirlar. +- Yazilim filtresi PI'nin yerine gecmez. Yalnizca 5 Hz WLS clock-bias + olcumunde Doppler clock-drift prediction, bounded innovation ve aykiri + olcum reddi yapar. Ilk yerlesmede commit'ler coarse ve PPS invalid tutulur. + Periyodik background watchdog iki saniye yeni referans gelmezse cikis + validligini holdover ile kapatir; fakat ayni referans geri gelirse Doppler + prediction ve donanim PI state'i 30 saniyeye kadar korunur. Referans SVID + degisimi veya 30 saniyeyi asan bosluk coarse re-anchor yapar. PPS v2.2 + holdover aktifken normal PI commit'ini uygulamadigi icin recovery'de butun + shadow word'ler once yazilir, holdover commit toggle'inin hemen oncesinde + birakilir ve ilk taze olcum PI'ya girer. Fiziksel PPS held rate ile + kesintisiz yurur; `STATUS.VALID` PI apply'dan yalniz bounded + synchronizer/commit-pipeline gecikmesi kadar once gorunebilir. +- PPS v2.2 `LOOP_CONFIG` register'i `0x88` adresindedir. Eski v2.1 bitstream + algilanirsa yazilim bu register'a dokunmaz; dinamik Q2.46 `Kp/Ki/max-slew` + yazimi mevcut register'larla calismaya devam eder. - 125 MHz fiziksel kenar grid'i 8 ns'dir. Generated VCU118 ve Genesys2 top'lari `gps_pps` output'unu expose eder. VCU118 icin PPS PMOD1/J53.8 (`R29`), Genesys2 icin PMOD JC10 (`AK28`) pinine atanmistir. Board testinde Gray/PCLK capture pipeline sabit offset'i ve clock-tree/I/O gecikmesi olculup kalibre edilmelidir. + +### PPS Sentetik Dogrulama (2026-07-25) + +Python modeli ve Linux C testi ayni Q32/Q46 PI, kalite histerezisi, Doppler +prediction, holdover ve lock politikasini kullanir. Ayrica donanim phase-IIR +kullanilmaz. Test matrisi temiz/320 ns WLS noise, 650 ns zayif geometri, +baslangicta 1.1 us noise sonra temiz sinyal, aralikli 900 ns noise burstleri, +uydu kaybi ve geri gelmesi, 15 saniyelik state-korumali ve 30 saniye ustu +coarse WLS kesintisi, Doppler kaybi, multipath +basamagi, profil sicramalari, +18 ppm slew siniri ve +25 ppm sinir disi +osilatoru kapsar. + +Monte Carlo sonucunda hedeflenen senaryolarin tamami lock/recovery sagladi. +Normal kart-noise senaryosunda 50/50 kosu lock oldu ve 125 MHz edge +quantization dahil bir saniyelik jitter p95 `13.62 ns` bulundu. Baslangicta +yuksek noise sonra temiz sinyal senaryosunda 30/30, noise-burst senaryosunda +30/30, iki uydu-kaybi araligi bulunan senaryoda 30/30 recovery goruldu. +`+18 ppm` 30/30 yerlesirken `+25 ppm`, `20 ppm` clamp disinda oldugu icin +0/10 lock uretti; yani saturation yanlis-valid olusturmadi. Bunlar sentetik +loop/jitter sonuclaridir; mutlak UTC/GPS faz dogrulugu icin karttaki sabit +capture/I/O offset'i ve gercek multipath halen osiloskop/time-interval olcumu +gerektirir. + +### PPS Kart Dogrulamasi (2026-07-25, Genesys2) + +- Kartta okunan RTL surumu `0x00020200`; dinamik `LOOP_CONFIG=0x08800800` + mailbox'i ve diagnostic snapshot calisti. +- Eski `16.368 MHz` zaman donusumu 6 saniyede yaklasik `6.97 us` hata + uretiyordu. Fiziksel `16.367981 MHz` donusumu ve 200 ms transmit-grid snap + sonrasinda 6 saniyelik mikro-saniye sawtooth kayboldu. +- Fractional capture duzeltmesi kartta epoch'a gore `0..64 ns` uygulandi + (son deger 8 ns grid'ine yuvarlanir). Duzeltmeden onceki uzun lock + penceresinde internal commit residual'i yaklasik `20.9 ns` median mutlak ve + `62.8 ns` p95 idi. Duzeltmeden sonraki precision penceresinde median mutlak + residual yaklasik `17 ns`, RMS `26 ns` ve p95 yaklasik `55 ns` oldu. +- Ilk kart yaziliminda iki saniyeyi asan normal coherent-WLS scheduling + bosluklari filtreyi coarse resetliyordu; bu PPS'in periyodik olarak + 4--5 update invalid kalmasina yol acti. Esigi validlik watchdog'undan + ayirdiktan sonra `seq=10551 -> 10561` araliginda `pulse=2174 -> 2186` + ile yaklasik 12 saniyelik gercek kart boslugu goruldu. Buna ragmen + `mode=tracking`, `locked=1`, status `0x80f` ve PI integrator kesintisiz + kaldi; coarse commit veya faz basamagi olusmadi. +- Her WLS commit'inin loglandigi `pps-fullrate` tanisinda `seq=16090..17631` + arasinda 1514 commit izlendi. Ilk lock'tan sonra coarse veya unlock olmadi. + `seq>=16250` icin 1355 locked residual'in median mutlak degeri `17.29 ns`, + RMS'i `34.17 ns`, p95'i `52.76 ns`, p99'u `67.24 ns` oldu. Bu sayilar + fiziksel PPS pin jitter'i degil, 5 Hz WLS commit residual'idir. +- Ayni tanida quality'nin robust'a dustugu tek bir commit'te `750.93 ns` + residual goruldu ve bir sonraki 200 ms commit'te normale dondu. Robust + `1.024 us` release esigi gereksiz pulse kaybini onledi. Daha once precision + `512 ns` release ile gorulen tek kisa unlock nedeniyle uretim precision + esigi de `1.024 us` yapildi; acquisition kosulu bes ardisik `<=64 ns` + olarak kaldi. Son `pps-final` kart okumasinda `LOOP_CONFIG=0x05800800`, + `CONTROL=0x3`, valid/locked status `0x80f` goruldu ve kontrollu holdover + denemesinden sonra ilk normal commit ile control/valid geri geldi. +- Bu `hw_phase`, 5 Hz WLS reference commit residual'idir ve dogrudan fiziksel + 1PPS kenar jitter'i degildir. Ayni pencerede proportional rate terimi + yaklasik `9.6 ppb RMS` idi; sentetik 125 MHz edge modeli kart-noise + senaryosunda bir saniyelik jitter p95 `13.62 ns` veriyor. `15--20 ns` + fiziksel hedef makuldur fakat pin uzerinde scope/TIC ile GPS-disiplinli + harici referansa karsi olculmeden saglandi diye raporlanmamalidir. diff --git a/soc/software/gps_system/header/gps/NavEquations.h b/soc/software/gps_system/header/gps/NavEquations.h index 131aa72..aec4b49 100644 --- a/soc/software/gps_system/header/gps/NavEquations.h +++ b/soc/software/gps_system/header/gps/NavEquations.h @@ -9,6 +9,7 @@ typedef struct gps_nav_solution { double ecef_m[3]; double clock_bias_m; + double clock_bias_variance_m2; double velocity_ecef_mps[3]; double velocity_covariance_ecef_mps2[3][3]; double clock_drift_mps; diff --git a/soc/software/gps_system/header/gps/Navigation.h b/soc/software/gps_system/header/gps/Navigation.h index 20d18dc..76edf00 100644 --- a/soc/software/gps_system/header/gps/Navigation.h +++ b/soc/software/gps_system/header/gps/Navigation.h @@ -37,8 +37,21 @@ #define WN_INDEX 2 #define POSITION_VECTOR_COL 3 #define MAX_24_BIT_INT 0x00800000 - // ADC sample frekansi - #define SAMPLES_PER_CODE 16368.0 + /* + * RF ADC sample rate used for time and pseudorange conversion. + * + * The acquisition RTL buffers an integer 16368 samples per nominal C/A + * code period, but the Genesys2/VCU118 RF reference is 16.367981 MHz. + * Using 16368.000 samples/ms as a frequency creates a 1.161 ppm timing + * ramp which is reset by every six-second HOW anchor (about 6.97 us + * peak-to-peak). Keep the physical clock rate distinct from the integer + * acquisition-buffer length. + */ + #ifndef GPS_RF_SAMPLE_RATE_HZ + #define GPS_RF_SAMPLE_RATE_HZ 16367981.0 + #endif + #define GPS_RF_SAMPLES_PER_MILLISECOND (GPS_RF_SAMPLE_RATE_HZ / 1000.0) + #define SAMPLES_PER_CODE GPS_RF_SAMPLES_PER_MILLISECOND #define CODE_FREQ_BASIS 1.023E+6 diff --git a/soc/software/gps_system/header/gps/gps_pps.h b/soc/software/gps_system/header/gps/gps_pps.h index c02b31b..8e5eb5c 100644 --- a/soc/software/gps_system/header/gps/gps_pps.h +++ b/soc/software/gps_system/header/gps/gps_pps.h @@ -4,11 +4,40 @@ #include #include +#include "gps/gps_pps_quality.h" +#include "gps/gps_pps_time_filter.h" + #define GPS_PPS_CLOCK_HZ 125000000u +typedef struct { + uint32_t status; + uint32_t active_sequence; + uint32_t pulse_count; + int64_t phase_error_q32; + int64_t integrator_q32; + int64_t rate_word_q32; +} gps_pps_diagnostics_t; + void gps_pps_init(unsigned int nav_solve_tick); +void gps_pps_service(void); bool gps_pps_is_locked(void); +bool gps_pps_set_loop_profile(gps_pps_profile_t profile); +void gps_pps_set_holdover(bool holdover); +void gps_pps_reset_time_filter(void); +bool gps_pps_read_diagnostics(gps_pps_diagnostics_t* diagnostics); bool gps_pps_read_capture(unsigned int channel, uint64_t* counter, uint32_t* sequence); -bool gps_pps_commit_solution(uint64_t epoch_counter, int gps_week, double receiver_tow_sec); +gps_pps_time_filter_result_t gps_pps_filter_clock_bias( + uint64_t epoch_counter, + double measured_clock_bias_m, + bool clock_drift_valid, + double measured_clock_drift_mps, + double tdop, + double* filtered_clock_bias_m_out, + double* innovation_m_out); +bool gps_pps_commit_solution( + uint64_t epoch_counter, + int gps_week, + double receiver_tow_sec, + bool force_coarse); #endif diff --git a/soc/software/gps_system/header/gps/gps_pps_quality.h b/soc/software/gps_system/header/gps/gps_pps_quality.h new file mode 100644 index 0000000..a6e95bf --- /dev/null +++ b/soc/software/gps_system/header/gps/gps_pps_quality.h @@ -0,0 +1,62 @@ +#ifndef GPS_PPS_QUALITY_H +#define GPS_PPS_QUALITY_H + +#include + +typedef enum { + GPS_PPS_PROFILE_HOLDOVER = 0, + GPS_PPS_PROFILE_ROBUST, + GPS_PPS_PROFILE_PRECISION, + GPS_PPS_PROFILE_ACQUIRE +} gps_pps_profile_t; + +typedef struct { + double tdop; + double gdop; + double clock_bias_sigma_m; + double mean_cn0_dbhz; + double minimum_cn0_dbhz; + double chi_square_ratio; + double velocity_residual_rms_mps; + unsigned int used_satellites; + bool solution_valid; + bool residual_test_valid; + bool velocity_valid; + bool fde_applied; +} gps_pps_quality_metrics_t; + +typedef struct { + gps_pps_profile_t profile; + gps_pps_profile_t candidate; + unsigned int candidate_updates; +} gps_pps_quality_selector_t; + +typedef struct { + double natural_frequency_hz; + double damping; + double max_slew_ppm; + unsigned int lock_acquire_ticks; + unsigned int lock_release_ticks; + unsigned int lock_acquire_updates; + double software_bias_gain; + unsigned int software_settle_updates; +} gps_pps_loop_profile_t; + +void gps_pps_quality_selector_init(gps_pps_quality_selector_t* selector); + +/* + * Select a bounded loop profile from timing geometry, code/carrier quality + * and integrity. Upgrades require sustained evidence and degradations require + * three consecutive observations. A single rejected WLS epoch is skipped + * without resetting the loop; sustained loss enters holdover, while the + * independent reference watchdog remains the final stale-data guard. + */ +gps_pps_profile_t gps_pps_quality_selector_update( + gps_pps_quality_selector_t* selector, + const gps_pps_quality_metrics_t* metrics, + bool* profile_changed_out); + +gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile); +const char* gps_pps_profile_name(gps_pps_profile_t profile); + +#endif diff --git a/soc/software/gps_system/header/gps/gps_pps_time_filter.h b/soc/software/gps_system/header/gps/gps_pps_time_filter.h new file mode 100644 index 0000000..96c8eb5 --- /dev/null +++ b/soc/software/gps_system/header/gps/gps_pps_time_filter.h @@ -0,0 +1,82 @@ +#ifndef GPS_PPS_TIME_FILTER_H +#define GPS_PPS_TIME_FILTER_H + +#include +#include + +typedef enum { + GPS_PPS_TIME_FILTER_REJECTED = 0, + GPS_PPS_TIME_FILTER_COARSE, + GPS_PPS_TIME_FILTER_TRACKING +} gps_pps_time_filter_result_t; + +typedef struct { + uint64_t epoch_counter; + uint64_t drift_anchor_counter; + double clock_bias_m; + double clock_drift_mps; + double previous_measurement_m; + double drift_anchor_measurement_m; + unsigned int accepted_updates; + bool initialized; + bool drift_initialized; +} gps_pps_time_filter_t; + +void gps_pps_time_filter_init(gps_pps_time_filter_t* filter); + +/* + * Keep the current WLS datum satellite while it remains in the coherent + * snapshot. Fall back to the earliest capture only when that satellite is + * gone. Stable selection avoids artificial clock-bias jumps as satellites + * enter the solution. + */ +bool gps_pps_select_reference( + const unsigned int satellite_ids[], + const double capture_times[], + unsigned int count, + bool preferred_valid, + unsigned int preferred_id, + unsigned int* reference_index_out); + +/* + * The position WLS observations are formed from capture_i - min(capture). + * That subtraction removes the common receiver-clock offset and drift before + * WLS. The fitted clock-bias datum is STARTOFFSET*c - reference_range (plus + * the reference satellite clock correction), so its derivative is + * -reference_range_rate + c*reference_satellite_clock_drift. + */ +bool gps_pps_reference_bias_drift( + const double receiver_ecef_m[3], + const double receiver_velocity_ecef_mps[3], + const double reference_satellite_ecef_m[3], + const double reference_satellite_velocity_ecef_mps[3], + double reference_satellite_clock_drift_sps, + double* clock_bias_drift_mps_out); + +/* + * Smooth the pseudorange WLS receiver-clock bias without lagging its large + * linear oscillator drift. The Doppler WLS clock-drift state predicts the + * bias at the new capture epoch; only the residual bias noise is low-passed. + * If Doppler is temporarily unavailable, a bounded, slower finite-difference + * drift estimate keeps the filter usable in the robust profile. + * + * COARSE is returned during initial settling, a reference-datum change, or + * after a gap too long for bounded Doppler prediction. The independent + * reference watchdog may suppress PPS validity during a shorter gap without + * destroying filter/PI state. The caller must keep PPS output invalid while + * COARSE is returned. + */ +gps_pps_time_filter_result_t gps_pps_time_filter_update( + gps_pps_time_filter_t* filter, + uint64_t epoch_counter, + double counter_frequency_hz, + double measured_clock_bias_m, + bool clock_drift_valid, + double measured_clock_drift_mps, + double tdop, + double bias_gain, + unsigned int settle_updates, + double* filtered_clock_bias_m_out, + double* innovation_m_out); + +#endif diff --git a/soc/software/gps_system/header/gps/gps_time_anchor.h b/soc/software/gps_system/header/gps/gps_time_anchor.h index 19734f2..f0877c6 100644 --- a/soc/software/gps_system/header/gps/gps_time_anchor.h +++ b/soc/software/gps_system/header/gps/gps_time_anchor.h @@ -36,6 +36,40 @@ bool gps_time_from_sample_anchor(int anchor_week, double* event_tow_sec_out, double* elapsed_sec_out); +/* + * Convert a periodic tracking event to GPS transmit time. The RF sample + * interval identifies and validates the event number; the result is then + * snapped to the known navigation-event grid. This removes the + * satellite-range-rate term present in receive-time sample intervals. + */ +bool gps_time_from_periodic_sample_anchor( + int anchor_week, + double anchor_tow_sec, + uint32_t anchor_raw_sample, + double anchor_corrected_sample, + uint32_t event_raw_sample, + double event_corrected_sample, + double samples_per_second, + double max_age_sec, + double event_period_sec, + double max_snap_error_sec, + int* event_week_out, + double* event_tow_sec_out, + double* elapsed_sec_out); + +/* + * Refer a counter captured at the first RF sample edge after a code boundary + * back to that fractional code boundary. fractional_sample_delay is + * raw_abs_sample - corrected_abs_sample and must be in [0, 1]. + */ +bool gps_time_correct_capture_counter( + uint64_t capture_counter, + double fractional_sample_delay, + double samples_per_second, + double counter_frequency_hz, + uint64_t* corrected_counter_out, + double* applied_correction_sec_out); + /* Validate that two decoded HOW/sample anchors describe the same monotonic * receiver timeline. This keeps a single transient HOW from replacing a * healthy anchor while still permitting normal week rollover. */ diff --git a/soc/software/gps_system/src/gps/NavEquations.c b/soc/software/gps_system/src/gps/NavEquations.c index fc699dd..4d15bf2 100644 --- a/soc/software/gps_system/src/gps/NavEquations.c +++ b/soc/software/gps_system/src/gps/NavEquations.c @@ -1208,6 +1208,28 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS covariance_between_axes(position_covariance, enu_axes[row], enu_axes[col]); } } + double clock_covariance_scale = 1.0; + if(degrees_of_freedom > 0u) + { + clock_covariance_scale = + fmax(1.0, weighted_residual_sum / (double)degrees_of_freedom); + } + solution_out->clock_bias_variance_m2 = + clock_covariance_scale * position_covariance->matrix[3][3]; + if(!isfinite(solution_out->clock_bias_variance_m2) || + (solution_out->clock_bias_variance_m2 < 0.0)) + { + GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, + "clock-bias covariance rejected variance=%e scale=%e\r\n", + solution_out->clock_bias_variance_m2, clock_covariance_scale); + free_matrix(position_covariance); + free_matrix(A); + free_matrix(posVec); + free(absoluteWeights); + free(phi_lambda_h); + free(topcent); + return false; + } free_matrix(position_covariance); free_matrix(A); diff --git a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c index 2277756..bc0277c 100644 --- a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c +++ b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c @@ -92,6 +92,7 @@ static gps_external_irq_handler_t gps_external_irq_handler = NULL; #define GPS_NAV_HOLDOFF_LOCAL_TICKS \ (((uint64_t)BSP_CLINT_TIMEBASE_HZ * GPS_NAV_EPOCH_HOLDOFF_MS) / 1000u) #define GPS_TRACK_DOPPLER_SMOOTH_ALPHA 0.05 +#define GPS_NAV_TOW_SNAP_TOLERANCE_SEC 0.005 _Static_assert(GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS < (GPS_NAV_SOLVE_PERIOD_MS / 2u), @@ -1161,7 +1162,7 @@ static bool gps_how_candidate_consistent( first_raw_sample, first_corrected_sample, second_week, second_tow_sec, second_raw_sample, second_corrected_sample, - SAMPLES_PER_CODE * 1000.0, + GPS_RF_SAMPLE_RATE_HZ, GPS_HOW_CONTINUITY_MAX_GAP_SEC, GPS_HOW_CONTINUITY_TOLERANCE_SEC); } @@ -1388,17 +1389,20 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event) int tow_week = 0; double tow_sec = 0.0; double tow_delta_sec = 0.0; - if(!gps_time_from_sample_anchor(node->gps_how_tow.week, - node->gps_how_tow.sec, - node->gps_how_abs_sample, - node->gps_how_corrected_abs_sample, - event->abs_sample_raw, - event->corrected_abs_sample, - SAMPLES_PER_CODE * 1000.0, - GPS_HOW_ANCHOR_MAX_AGE_SEC, - &tow_week, - &tow_sec, - &tow_delta_sec)) + if(!gps_time_from_periodic_sample_anchor( + node->gps_how_tow.week, + node->gps_how_tow.sec, + node->gps_how_abs_sample, + node->gps_how_corrected_abs_sample, + event->abs_sample_raw, + event->corrected_abs_sample, + GPS_RF_SAMPLE_RATE_HZ, + GPS_HOW_ANCHOR_MAX_AGE_SEC, + (double)GPS_NAV_SOLVE_PERIOD_MS / 1000.0, + GPS_NAV_TOW_SNAP_TOLERANCE_SEC, + &tow_week, + &tow_sec, + &tow_delta_sec)) { GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, "nav solve rejected by stale/invalid HOW anchor: trk=%u prn=%u anchor_tow=%.3f anchor_abs=%u event_abs=%u\r\n", @@ -1912,6 +1916,7 @@ void gps_background_task(void) gps_process_isr_events(); if(gps_take_timer_maintenance_pending()) { + gps_pps_service(); gps_reconcile_stopped_tracking_channels(); gps_mark_stale_measurements(); gps_check_acq_stuck_busy(); diff --git a/soc/software/gps_system/src/gps/gps_ip.c b/soc/software/gps_system/src/gps/gps_ip.c index d903315..72f0f64 100644 --- a/soc/software/gps_system/src/gps/gps_ip.c +++ b/soc/software/gps_system/src/gps/gps_ip.c @@ -10,6 +10,7 @@ #include "gps/gps_interrupt_handlers.h" #include "gps/gps_nav_epoch.h" #include "gps/gps_pps.h" +#include "gps/gps_time_anchor.h" #include "gps/defines.h" #include @@ -61,6 +62,17 @@ volatile gps_ephem_ll_t* tracking_node = NULL; static volatile bool gps_initial_position_valid = false; static volatile bool gps_ionoutc_valid = false; static ionoutc_t gps_last_ionoutc; +static gps_pps_quality_selector_t gps_pps_quality_selector; +static gps_pps_profile_t gps_pps_applied_profile = GPS_PPS_PROFILE_HOLDOVER; +static unsigned int gps_pps_status_log_decimator; +static bool gps_pps_reference_svid_valid; +static unsigned int gps_pps_reference_svid; +static bool gps_pps_filter_reference_svid_valid; +static unsigned int gps_pps_filter_reference_svid; + +#if !defined(GPS_PPS_STATUS_LOG_INTERVAL) || (GPS_PPS_STATUS_LOG_INTERVAL < 1) +#error "GPS_PPS_STATUS_LOG_INTERVAL must be at least one" +#endif volatile uint64_t gps_systick_ticks = 0; @@ -658,6 +670,7 @@ static unsigned int gps_collect_completed_epoch_locked( gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT], unsigned char snapshot_ips[GPS_TRCK_IP_COUNT], uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT], + double snapshot_pps_sample_delays[GPS_TRCK_IP_COUNT], uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT], uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT], uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT], @@ -690,6 +703,9 @@ static unsigned int gps_collect_completed_epoch_locked( snapshot_ips[count] = ip_no; snapshot_pps_counters[count] = epoch->pps_counter[ip_no]; + snapshot_pps_sample_delays[count] = + (double)epoch->raw_sample[ip_no] - + epoch->corrected_abs_sample[ip_no]; snapshot_pps_sequences[count] = epoch->pps_capture_sequence[ip_no]; snapshot_local_timestamps[count] = @@ -707,6 +723,7 @@ static unsigned int gps_select_coherent_nav_snapshot( gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT], unsigned char snapshot_ips[GPS_TRCK_IP_COUNT], uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT], + double snapshot_pps_sample_delays[GPS_TRCK_IP_COUNT], uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT], uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT], uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT], @@ -754,6 +771,8 @@ static unsigned int gps_select_coherent_nav_snapshot( snapshot_ips[destination] = snapshot_ips[source]; snapshot_pps_counters[destination] = snapshot_pps_counters[source]; + snapshot_pps_sample_delays[destination] = + snapshot_pps_sample_delays[source]; snapshot_pps_sequences[destination] = snapshot_pps_sequences[source]; snapshot_local_timestamps[destination] = @@ -799,12 +818,15 @@ static uint64_t gps_nav_median_local_timestamp( static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, const uint64_t pps_counters[GPS_TRCK_IP_COUNT], + const double pps_sample_delays[GPS_TRCK_IP_COUNT], const uint32_t pps_capture_sequences[GPS_TRCK_IP_COUNT], const gps_tracking_measurement_t* measurements, unsigned int satellite_count, gps_nav_solution_t* solution_out) { - if((ephemeris == NULL) || (measurements == NULL) || + if((ephemeris == NULL) || (pps_counters == NULL) || + (pps_sample_delays == NULL) || + (pps_capture_sequences == NULL) || (measurements == NULL) || (solution_out == NULL) || (satellite_count < GPS_NAV_MIN_WLS_SATELLITES)) { @@ -829,6 +851,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, ionoutc_t ionoutc_snapshot; const ionoutc_t* ionoutc = gps_get_ionoutc(&ionoutc_snapshot) ? &ionoutc_snapshot : NULL; bool ok = false; + bool pps_clock_bias_drift_valid = false; + double pps_clock_bias_drift_mps = 0.0; gps_nav_solution_t solution; memset(&solution, 0, sizeof(solution)); memset(solution_out, 0, sizeof(*solution_out)); @@ -866,16 +890,30 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, satellitePositions, velocities); } - double minimum = psTVLtime[0]; - unsigned int minimum_index = 0u; - for(unsigned int i = 1u ; i < satellite_count ; i++) + unsigned int satellite_ids[GPS_TRCK_IP_COUNT]; + for(unsigned int i = 0u; i < satellite_count; i++) { - if(psTVLtime[i] < minimum) - { - minimum = psTVLtime[i]; - minimum_index = i; - } + satellite_ids[i] = ephemeris[i].SVID; } + unsigned int reference_index = 0u; + if(!gps_pps_select_reference( + satellite_ids, psTVLtime, satellite_count, + gps_pps_reference_svid_valid, gps_pps_reference_svid, + &reference_index)) + { + goto cleanup; + } + double reference_capture_time = psTVLtime[reference_index]; + uint64_t pps_reference_counter = 0u; + double pps_capture_correction_sec = 0.0; + bool pps_reference_counter_valid = + gps_time_correct_capture_counter( + pps_counters[reference_index], + pps_sample_delays[reference_index], + GPS_RF_SAMPLE_RATE_HZ, + (double)GPS_PPS_CLOCK_HZ, + &pps_reference_counter, + &pps_capture_correction_sec); preciseTxTimes = create_matrix(1, satellite_count, "preciseTxTimes"); localPseudoRanges = create_matrix(1, satellite_count, "localPseudoRanges"); @@ -894,7 +932,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, for(unsigned int i = 0 ; i < satellite_count ; i++) { - preciseTxTimes->matrix[0][i] = psTVLtime[i] - minimum + STARTOFFSET; + preciseTxTimes->matrix[0][i] = + psTVLtime[i] - reference_capture_time + STARTOFFSET; localPseudoRanges->matrix[0][i] = preciseTxTimes->matrix[0][i] * (SPEED_OF_LIGHT / 1000.0); GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "psTVLtime=%.10lf preciseTx=%.10lf pseudo=%.10lf clockCorr=%.10lf corrected=%.10lf\r\n", @@ -957,6 +996,14 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, solution.velocity_ecef_mps[1] + solution.velocity_ecef_mps[2] * solution.velocity_ecef_mps[2]); + pps_clock_bias_drift_valid = + gps_pps_reference_bias_drift( + solution.ecef_m, + solution.velocity_ecef_mps, + satellite_positions_array[reference_index], + satellite_velocities_array[reference_index], + clockDrift[reference_index], + &pps_clock_bias_drift_mps); } } if(ok && solution.fde_applied) @@ -971,27 +1018,221 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, } } bool pps_reference_used = ok && - ((solution.used_row_mask & (1u << minimum_index)) != 0u); - if(ok && solution.solution_valid && pps_reference_used && - (pps_capture_sequences[minimum_index] != 0u)) + ((solution.used_row_mask & (1u << reference_index)) != 0u); + if(ok && solution.solution_valid && pps_reference_used) { + gps_pps_reference_svid = ephemeris[reference_index].SVID; + gps_pps_reference_svid_valid = true; + } + else if(ok && !pps_reference_used) + { + /* Select a new datum on the next WLS solve if FDE rejected this one. */ + gps_pps_reference_svid_valid = false; + } + gps_pps_quality_metrics_t pps_quality = {0}; + pps_quality.solution_valid = ok && solution.solution_valid; + if(pps_quality.solution_valid) + { + double cn0_sum = 0.0; + double minimum_cn0 = INFINITY; + unsigned int cn0_count = 0u; + for(unsigned int row = 0u; row < satellite_count; row++) + { + if(((solution.used_row_mask & (1u << row)) != 0u) && + isfinite(solution.cn0_dbhz[row])) + { + cn0_sum += solution.cn0_dbhz[row]; + minimum_cn0 = fmin(minimum_cn0, solution.cn0_dbhz[row]); + cn0_count++; + } + } + pps_quality.tdop = solution.tdop; + pps_quality.gdop = solution.gdop; + pps_quality.clock_bias_sigma_m = + sqrt(solution.clock_bias_variance_m2); + pps_quality.mean_cn0_dbhz = + (cn0_count != 0u) ? (cn0_sum / (double)cn0_count) : NAN; + pps_quality.minimum_cn0_dbhz = + (cn0_count != 0u) ? minimum_cn0 : NAN; + pps_quality.chi_square_ratio = solution.chi_square_ratio; + pps_quality.velocity_residual_rms_mps = + solution.velocity_valid ? + solution.velocity_residual_rms_mps : 0.0; + pps_quality.used_satellites = solution.used_satellite_count; + pps_quality.residual_test_valid = solution.residual_test_valid; + pps_quality.velocity_valid = solution.velocity_valid; + pps_quality.fde_applied = solution.fde_applied; + } + bool pps_profile_changed = false; + /* + * An invalid WLS epoch carries no usable quality evidence and is not fed + * into the profile selector. It is also never committed below. This + * lets isolated chi-square rejects pass without resetting a converged + * loop; continuous loss is independently caught by gps_pps_service()'s + * two-second reference watchdog. + */ + gps_pps_profile_t pps_profile = gps_pps_quality_selector.profile; + if(pps_quality.solution_valid) + { + pps_profile = gps_pps_quality_selector_update( + &gps_pps_quality_selector, &pps_quality, + &pps_profile_changed); + } + if(pps_profile == GPS_PPS_PROFILE_HOLDOVER) + { + gps_pps_set_holdover(true); + } + gps_pps_profile_t desired_loop_profile = + ((pps_profile != GPS_PPS_PROFILE_HOLDOVER) && + !gps_pps_is_locked()) ? + GPS_PPS_PROFILE_ACQUIRE : pps_profile; + if((desired_loop_profile != GPS_PPS_PROFILE_HOLDOVER) && + (desired_loop_profile != gps_pps_applied_profile) && + gps_pps_set_loop_profile(desired_loop_profile)) + { + gps_pps_applied_profile = desired_loop_profile; + } + if(pps_profile_changed) + { + GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV, + "PPS quality profile=%s tdop=%.2lf gdop=%.2lf " + "clock_sigma=%.2lf cn0_mean=%.1lf cn0_min=%.1lf " + "chi2_ratio=%.3lf velocity_rms=%.3lf\r\n", + gps_pps_profile_name(pps_profile), + pps_quality.tdop, pps_quality.gdop, + pps_quality.clock_bias_sigma_m, + pps_quality.mean_cn0_dbhz, + pps_quality.minimum_cn0_dbhz, + pps_quality.chi_square_ratio, + pps_quality.velocity_residual_rms_mps); + } + + if(ok && solution.solution_valid && pps_reference_used && + (pps_capture_sequences[reference_index] != 0u) && + pps_reference_counter_valid && + (pps_profile != GPS_PPS_PROFILE_HOLDOVER)) + { + unsigned int reference_svid = ephemeris[reference_index].SVID; + if(!gps_pps_filter_reference_svid_valid || + (gps_pps_filter_reference_svid != reference_svid)) + { + gps_pps_reset_time_filter(); + gps_pps_filter_reference_svid = reference_svid; + gps_pps_filter_reference_svid_valid = true; + GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV, + "PPS WLS datum re-anchor ref_svid=%u\r\n", + reference_svid); + } + double filtered_clock_bias_m = 0.0; + double clock_innovation_m = 0.0; + gps_pps_time_filter_result_t filter_result = + gps_pps_filter_clock_bias( + pps_reference_counter, + solution.clock_bias_m, + pps_clock_bias_drift_valid, + pps_clock_bias_drift_mps, + solution.tdop, + &filtered_clock_bias_m, + &clock_innovation_m); + if(filter_result == GPS_PPS_TIME_FILTER_REJECTED) + { + GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV, + "PPS clock observation rejected profile=%s bias=%.3lf " + "drift=%.3lf tdop=%.2lf\r\n", + gps_pps_profile_name(pps_profile), + solution.clock_bias_m, + solution.clock_drift_mps, + solution.tdop); + goto pps_anchor_done; + } /* The synthetic STARTOFFSET datum is absorbed by WLS b. Removing b/c * recovers the reference satellite's physical flight time; POSTPONE_TOW * is applied exactly once here, just as it is in satpos(). */ - double receiver_tow = ephemeris[minimum_index].tow.sec + POSTPONE_TOW + - (STARTOFFSET / 1000.0) - (solution.clock_bias_m / SPEED_OF_LIGHT); - if(!gps_pps_commit_solution(pps_counters[minimum_index], - ephemeris[minimum_index].tow.week, - receiver_tow)) + double receiver_tow = ephemeris[reference_index].tow.sec + POSTPONE_TOW + + (STARTOFFSET / 1000.0) - + (filtered_clock_bias_m / SPEED_OF_LIGHT); + /* + * Keep holdover asserted throughout initial settling and re-anchor + * commits. A coarse commit is accepted while held and clears lock. + * gps_pps_commit_solution() releases holdover immediately before a + * normal mailbox request so the first coherent recovery update is + * applied by PPS v2.2 without stepping the held physical timebase. + */ + if(filter_result == GPS_PPS_TIME_FILTER_COARSE) + { + gps_pps_set_holdover(true); + } + if(!gps_pps_commit_solution(pps_reference_counter, + ephemeris[reference_index].tow.week, + receiver_tow, + filter_result == GPS_PPS_TIME_FILTER_COARSE)) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "PPS anchor skipped: mailbox busy or invalid time\r\n"); } + gps_pps_status_log_decimator++; + if(gps_pps_status_log_decimator >= GPS_PPS_STATUS_LOG_INTERVAL) + { + gps_pps_status_log_decimator = 0u; + gps_pps_diagnostics_t pps_diagnostics; + bool pps_diagnostics_valid = + gps_pps_read_diagnostics(&pps_diagnostics); + double hardware_phase_ns = pps_diagnostics_valid ? + ((double)pps_diagnostics.phase_error_q32 / + 4294967296.0) * + (1000000000.0 / (double)GPS_PPS_CLOCK_HZ) : NAN; + double rate_command_ppm = pps_diagnostics_valid ? + ((double)pps_diagnostics.rate_word_q32 / + 4294967296.0) * 1000000.0 : NAN; + double integrator_ppm = pps_diagnostics_valid ? + ((double)pps_diagnostics.integrator_q32 / + 4294967296.0) * 1000000.0 : NAN; + GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV, + "PPS profile=%s mode=%s locked=%u tdop=%.2lf gdop=%.2lf " + "clock_sigma=%.2lf cn0=%.1lf/%.1lf innovation=%.2lfm " + "raw_bias=%.3lfm filtered_bias=%.3lfm " + "clock_drift=%.3lfmps bias_drift=%.3lfmps ref_svid=%u " + "capture_corr=%.2lfns " + "hw_phase=%.2lfns rate_cmd=%.3lfppm integ=%.3lfppm " + "pulse=%u seq=%u status=0x%03x\r\n", + gps_pps_profile_name(pps_profile), + (filter_result == GPS_PPS_TIME_FILTER_COARSE) ? + "coarse" : "tracking", + gps_pps_is_locked() ? 1u : 0u, + pps_quality.tdop, pps_quality.gdop, + pps_quality.clock_bias_sigma_m, + pps_quality.mean_cn0_dbhz, + pps_quality.minimum_cn0_dbhz, + clock_innovation_m, + solution.clock_bias_m, + filtered_clock_bias_m, + solution.clock_drift_mps, + pps_clock_bias_drift_mps, + ephemeris[reference_index].SVID, + pps_capture_correction_sec * 1.0e9, + hardware_phase_ns, rate_command_ppm, integrator_ppm, + pps_diagnostics_valid ? + pps_diagnostics.pulse_count : 0u, + pps_diagnostics_valid ? + pps_diagnostics.active_sequence : 0u, + pps_diagnostics_valid ? + pps_diagnostics.status : 0u); + } + } + else if(ok && solution.solution_valid && pps_reference_used && + (pps_capture_sequences[reference_index] != 0u) && + !pps_reference_counter_valid) + { + GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, + "PPS anchor rejected: invalid fractional capture correction " + "ref_svid=%u sample_delay=%.9lf\r\n", + ephemeris[reference_index].SVID, + pps_sample_delays[reference_index]); } else if(ok && solution.solution_valid && !pps_reference_used) { GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, "PPS anchor protected: FDE excluded pseudorange reference row=%u SVID=%u\r\n", - minimum_index, ephemeris[minimum_index].SVID); + reference_index, ephemeris[reference_index].SVID); } else if(ok && !solution.solution_valid) { @@ -999,6 +1240,7 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, "PPS/last_solution protected from invalid best-effort NAVSOL quality=%.1lf ratio=%.3lf\r\n", solution.quality_score, solution.chi_square_ratio); } +pps_anchor_done: cleanup: if(preciseTxTimes != NULL) free_matrix(preciseTxTimes); @@ -1140,6 +1382,7 @@ bool calculate_position(gps_nav_solution_t* solution_out, gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT]; unsigned char snapshot_ips[GPS_TRCK_IP_COUNT]; uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT]; + double snapshot_pps_sample_delays[GPS_TRCK_IP_COUNT]; uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT]; uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT]; uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT]; @@ -1186,6 +1429,7 @@ bool calculate_position(gps_nav_solution_t* solution_out, candidate_count = gps_collect_completed_epoch_locked( &completed_epoch, snapshot, snapshot_ips, snapshot_pps_counters, + snapshot_pps_sample_delays, snapshot_pps_sequences, snapshot_local_timestamps, snapshot_raw_samples, snapshot_measurements); @@ -1211,6 +1455,7 @@ bool calculate_position(gps_nav_solution_t* solution_out, { satellite_count = gps_select_coherent_nav_snapshot( snapshot, snapshot_ips, snapshot_pps_counters, + snapshot_pps_sample_delays, snapshot_pps_sequences, snapshot_local_timestamps, snapshot_raw_samples, snapshot_measurements, @@ -1267,6 +1512,7 @@ bool calculate_position(gps_nav_solution_t* solution_out, memset(&nav_solution, 0, sizeof(nav_solution)); uint64_t nav_software_start_cycles = gps_read_cycle_counter(); bool nav_ok = nav_software_from_snapshot(snapshot, snapshot_pps_counters, + snapshot_pps_sample_delays, snapshot_pps_sequences, snapshot_measurements, satellite_count, @@ -1486,6 +1732,13 @@ void gps_startup_initialization(unsigned int nsp_tick) GPS_NAV_ENABLE_FDE); gps_init_acq_tables(); gps_pps_init(nsp_tick); + gps_pps_quality_selector_init(&gps_pps_quality_selector); + gps_pps_applied_profile = GPS_PPS_PROFILE_HOLDOVER; + gps_pps_status_log_decimator = 0u; + gps_pps_reference_svid_valid = false; + gps_pps_reference_svid = 0u; + gps_pps_filter_reference_svid_valid = false; + gps_pps_filter_reference_svid = 0u; gps_ionoutc_valid = false; gps_last_ionoutc = (ionoutc_t){0}; gps_nav_epoch_reject_total = 0u; diff --git a/soc/software/gps_system/src/gps/gps_pps.c b/soc/software/gps_system/src/gps/gps_pps.c index d53ee51..cb2fff9 100644 --- a/soc/software/gps_system/src/gps/gps_pps.c +++ b/soc/software/gps_system/src/gps/gps_pps.c @@ -1,4 +1,5 @@ #include "gps/gps_pps.h" +#include "gps/gps_macros.h" #include #include @@ -39,6 +40,7 @@ typedef struct { volatile uint32_t version; /* 0x7c */ volatile uint32_t kp_q46_hi; /* 0x80 */ volatile uint32_t ki_q46_hi; /* 0x84 */ + volatile uint32_t loop_config; /* 0x88, PPS v2.2+ */ } gps_pps_regs_t; #if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201112L) @@ -46,23 +48,44 @@ _Static_assert(offsetof(gps_pps_regs_t, capture_counter_lo) == 0x14, "PPS captur _Static_assert(offsetof(gps_pps_regs_t, epoch_counter_lo) == 0x20, "PPS epoch offset mismatch"); _Static_assert(offsetof(gps_pps_regs_t, commit) == 0x44, "PPS commit offset mismatch"); _Static_assert(offsetof(gps_pps_regs_t, kp_q46_hi) == 0x80, "PPS coefficient offset mismatch"); +_Static_assert(offsetof(gps_pps_regs_t, loop_config) == 0x88, "PPS loop-config offset mismatch"); #endif enum { GPS_PPS_CONTROL_ENABLE = 1u << 0, GPS_PPS_CONTROL_TRACK = 1u << 1, + GPS_PPS_CONTROL_HOLDOVER = 1u << 2, GPS_PPS_CONTROL_CLEAR_STATUS = 1u << 3, GPS_PPS_STATUS_ALIGNED = 1u << 1, GPS_PPS_STATUS_COMMIT_BUSY = 1u << 4, + GPS_PPS_STATUS_DIAG_BUSY = 1u << 10, GPS_PPS_STATUS_LOCKED = 1u << 11, GPS_PPS_COMMIT_VALID = 1u << 0, GPS_PPS_COMMIT_COARSE = 1u << 1 }; +#define GPS_PPS_VERSION_ADAPTIVE_LOOP 0x00020200u +#define GPS_PPS_REFERENCE_TIMEOUT_TICKS (2u * GPS_PPS_CLOCK_HZ) + static volatile gps_pps_regs_t* const gps_pps = (volatile gps_pps_regs_t*)BSP_GPS_PPS_BASE; static uint32_t gps_pps_solution_sequence; +static uint32_t gps_pps_version; +static unsigned int gps_pps_nav_solve_tick; +static gps_pps_profile_t gps_pps_active_loop_profile; static bool gps_pps_force_coarse; +static bool gps_pps_reference_committed; +static bool gps_pps_holdover; +static uint64_t gps_pps_last_reference_counter; +static gps_pps_time_filter_t gps_pps_time_filter; + +static uint64_t gps_pps_read_u64( + volatile uint32_t* lo, volatile uint32_t* hi) +{ + uint32_t low_word = *lo; + uint32_t high_word = *hi; + return ((uint64_t)high_word << 32) | low_word; +} static void gps_pps_write_q46(volatile uint32_t* lo, volatile uint32_t* hi, int64_t value) { @@ -71,29 +94,86 @@ static void gps_pps_write_q46(volatile uint32_t* lo, volatile uint32_t* hi, int6 *hi = (uint32_t)(encoded >> 32); } -void gps_pps_init(unsigned int nav_solve_tick) +static void gps_pps_write_loop_profile(gps_pps_profile_t profile) { const double pi = 3.14159265358979323846; - const double natural_frequency_hz = 0.035; - const double damping = 0.9; - const double update_period_sec = (nav_solve_tick != 0u) ? - ((double)nav_solve_tick * 0.020) : 0.200; - const double wn = 2.0 * pi * natural_frequency_hz; + gps_pps_loop_profile_t parameters = gps_pps_loop_profile(profile); + const double update_period_sec = (gps_pps_nav_solve_tick != 0u) ? + ((double)gps_pps_nav_solve_tick * 0.020) : 0.200; + const double wn = 2.0 * pi * parameters.natural_frequency_hz; const double q46 = 70368744177664.0; - int64_t kp_q46 = (int64_t)llround((2.0 * damping * wn / GPS_PPS_CLOCK_HZ) * q46); + int64_t kp_q46 = (int64_t)llround( + (2.0 * parameters.damping * wn / GPS_PPS_CLOCK_HZ) * q46); int64_t ki_q46 = (int64_t)llround((wn * wn * update_period_sec / GPS_PPS_CLOCK_HZ) * q46); + uint32_t max_slew_q32 = (uint32_t)llround( + parameters.max_slew_ppm * 1.0e-6 * 4294967296.0); - gps_pps->control = 0u; gps_pps_write_q46(&gps_pps->kp_q46_lo, &gps_pps->kp_q46_hi, kp_q46); gps_pps_write_q46(&gps_pps->ki_q46_lo, &gps_pps->ki_q46_hi, ki_q46); - gps_pps->max_slew_q32 = 85899u; /* 20 ppm of one Q32 tick per local clock. */ + gps_pps->max_slew_q32 = max_slew_q32; + if(gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP) + { + gps_pps->loop_config = + ((parameters.lock_acquire_ticks & 0xffu) << 8) | + ((parameters.lock_release_ticks & 0xffu) << 16) | + ((parameters.lock_acquire_updates & 0xffu) << 24); + } +} + +void gps_pps_init(unsigned int nav_solve_tick) +{ + gps_pps_nav_solve_tick = nav_solve_tick; + gps_pps->control = 0u; + gps_pps_version = gps_pps->version; + gps_pps_write_loop_profile(GPS_PPS_PROFILE_ROBUST); + gps_pps_active_loop_profile = GPS_PPS_PROFILE_ROBUST; /* A GDB CPU reset does not reset the tracking-clock PPS state. Continue * its mailbox sequence and force the first new solution to re-anchor the * timebase instead of feeding a new epoch into the previous PI state. */ - gps_pps_solution_sequence = gps_pps->active_sequence; + gps_pps_diagnostics_t initial_diagnostics; + if(gps_pps_read_diagnostics(&initial_diagnostics)) + { + gps_pps_solution_sequence = initial_diagnostics.active_sequence; + } + else + { + gps_pps_solution_sequence = gps_pps->active_sequence; + } gps_pps_force_coarse = true; + gps_pps_reference_committed = false; + gps_pps_holdover = true; + gps_pps_last_reference_counter = 0u; + gps_pps_time_filter_init(&gps_pps_time_filter); gps_pps->control = GPS_PPS_CONTROL_ENABLE | GPS_PPS_CONTROL_TRACK | + GPS_PPS_CONTROL_HOLDOVER | GPS_PPS_CONTROL_CLEAR_STATUS; + GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV, + "PPS init rtl=0x%08x adaptive=%u loop_config=0x%08x " + "clock_hz=%u update_ticks=%u\r\n", + gps_pps_version, + gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ? 1u : 0u, + gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ? + gps_pps->loop_config : 0u, + GPS_PPS_CLOCK_HZ, gps_pps_nav_solve_tick); +} + +void gps_pps_service(void) +{ + if(!gps_pps_reference_committed || gps_pps_holdover) + { + return; + } + + /* Reading LO snapshots HI in hardware, so this is a coherent 64-bit + * free-running counter read over the 32-bit APB interface. */ + uint32_t now_lo = gps_pps->raw_counter_lo; + uint32_t now_hi = gps_pps->raw_counter_hi; + uint64_t now = ((uint64_t)now_hi << 32) | now_lo; + if((now - gps_pps_last_reference_counter) > + (uint64_t)GPS_PPS_REFERENCE_TIMEOUT_TICKS) + { + gps_pps_set_holdover(true); + } } bool gps_pps_is_locked(void) @@ -101,6 +181,70 @@ bool gps_pps_is_locked(void) return (gps_pps->status & GPS_PPS_STATUS_LOCKED) != 0u; } +bool gps_pps_read_diagnostics(gps_pps_diagnostics_t* diagnostics) +{ + if((diagnostics == NULL) || + ((gps_pps->status & GPS_PPS_STATUS_DIAG_BUSY) != 0u)) + { + return false; + } + + gps_pps->diag_snapshot = 1u; + bool diag_started = false; + for(unsigned int attempt = 0u; attempt < 1000u; attempt++) + { + uint32_t status = gps_pps->status; + if((status & GPS_PPS_STATUS_DIAG_BUSY) != 0u) + { + diag_started = true; + } + /* + * The 125 MHz snapshot/ack can complete between two CPU MMIO + * reads. In that legitimate case DIAG_BUSY is never observed. + * Sixteen ordered APB reads are far longer than the request, + * snapshot, and two-flop return path; accepting a clear status + * after that delay still guarantees that the frozen group is new. + */ + else if(diag_started || (attempt >= 16u)) + { + diagnostics->status = status; + diagnostics->active_sequence = gps_pps->active_sequence; + diagnostics->pulse_count = gps_pps->pulse_count; + diagnostics->phase_error_q32 = (int64_t)gps_pps_read_u64( + &gps_pps->phase_error_lo, &gps_pps->phase_error_hi); + diagnostics->integrator_q32 = (int64_t)gps_pps_read_u64( + &gps_pps->integrator_lo, &gps_pps->integrator_hi); + diagnostics->rate_word_q32 = (int64_t)gps_pps_read_u64( + &gps_pps->rate_word_lo, &gps_pps->rate_word_hi); + return true; + } + } + return false; +} + +bool gps_pps_set_loop_profile(gps_pps_profile_t profile) +{ + if((profile == GPS_PPS_PROFILE_HOLDOVER) || + ((gps_pps->status & GPS_PPS_STATUS_COMMIT_BUSY) != 0u)) + { + return false; + } + gps_pps_write_loop_profile(profile); + gps_pps_active_loop_profile = profile; + return true; +} + +void gps_pps_set_holdover(bool holdover) +{ + uint32_t control = GPS_PPS_CONTROL_ENABLE | GPS_PPS_CONTROL_TRACK; + if(holdover) + { + control |= GPS_PPS_CONTROL_HOLDOVER; + } + gps_pps->control = control; + gps_pps_holdover = holdover; +} + bool gps_pps_read_capture(unsigned int channel, uint64_t* counter, uint32_t* sequence) { if((channel >= BSP_GPS_TRCK_COUNT) || (counter == NULL) || (sequence == NULL)) @@ -123,7 +267,37 @@ bool gps_pps_read_capture(unsigned int channel, uint64_t* counter, uint32_t* seq return true; } -bool gps_pps_commit_solution(uint64_t epoch_counter, int gps_week, double receiver_tow_sec) +gps_pps_time_filter_result_t gps_pps_filter_clock_bias( + uint64_t epoch_counter, + double measured_clock_bias_m, + bool clock_drift_valid, + double measured_clock_drift_mps, + double tdop, + double* filtered_clock_bias_m_out, + double* innovation_m_out) +{ + gps_pps_loop_profile_t parameters = + gps_pps_loop_profile(gps_pps_active_loop_profile); + return gps_pps_time_filter_update( + &gps_pps_time_filter, epoch_counter, (double)GPS_PPS_CLOCK_HZ, + measured_clock_bias_m, clock_drift_valid, + measured_clock_drift_mps, tdop, + parameters.software_bias_gain, + parameters.software_settle_updates, + filtered_clock_bias_m_out, innovation_m_out); +} + +void gps_pps_reset_time_filter(void) +{ + gps_pps_time_filter_init(&gps_pps_time_filter); + gps_pps_force_coarse = true; +} + +bool gps_pps_commit_solution( + uint64_t epoch_counter, + int gps_week, + double receiver_tow_sec, + bool force_coarse) { if(!isfinite(receiver_tow_sec)) { @@ -160,7 +334,7 @@ bool gps_pps_commit_solution(uint64_t epoch_counter, int gps_week, double receiv uint64_t gps_seconds = ((uint64_t)(unsigned int)gps_week * 604800ULL) + (uint64_t)whole_tow; uint32_t next_sequence = gps_pps_solution_sequence + 1u; - bool coarse = gps_pps_force_coarse || + bool coarse = force_coarse || gps_pps_force_coarse || ((gps_pps->status & GPS_PPS_STATUS_ALIGNED) == 0u); gps_pps->epoch_counter_lo = (uint32_t)epoch_counter; @@ -169,9 +343,24 @@ bool gps_pps_commit_solution(uint64_t epoch_counter, int gps_week, double receiv gps_pps->gps_seconds_hi = (uint32_t)(gps_seconds >> 32); gps_pps->gps_fraction_q32 = (uint32_t)fraction_q32; gps_pps->epoch_sequence = next_sequence; + /* + * PPS v2.2 deliberately rejects a normal PI update while HOLDOVER is + * asserted. Release it only after every shadow word is in place and + * immediately before toggling COMMIT, so the first coherent post-gap + * observation is applied instead of merely acknowledged. The physical + * PPS already runs from the held rate word, so this does not introduce a + * phase step; only the validity status precedes the PI apply by the + * bounded mailbox pipeline latency. + */ + if(!coarse && gps_pps_holdover) + { + gps_pps_set_holdover(false); + } gps_pps->commit = GPS_PPS_COMMIT_VALID | (coarse ? GPS_PPS_COMMIT_COARSE : 0u); gps_pps_solution_sequence = next_sequence; gps_pps_force_coarse = false; + gps_pps_reference_committed = true; + gps_pps_last_reference_counter = epoch_counter; return true; } diff --git a/soc/software/gps_system/src/gps/gps_pps_quality.c b/soc/software/gps_system/src/gps/gps_pps_quality.c new file mode 100644 index 0000000..a942f7f --- /dev/null +++ b/soc/software/gps_system/src/gps/gps_pps_quality.c @@ -0,0 +1,194 @@ +#include "gps/gps_pps_quality.h" + +#include +#include +#include +#include + +#define GPS_PPS_PROFILE_UPGRADE_UPDATES 8u +#define GPS_PPS_PROFILE_DEGRADE_UPDATES 3u + +static bool gps_pps_metrics_finite(const gps_pps_quality_metrics_t* metrics) +{ + return (metrics != NULL) && + isfinite(metrics->tdop) && + isfinite(metrics->gdop) && + isfinite(metrics->clock_bias_sigma_m) && + isfinite(metrics->mean_cn0_dbhz) && + isfinite(metrics->minimum_cn0_dbhz) && + isfinite(metrics->velocity_residual_rms_mps); +} + +static gps_pps_profile_t gps_pps_classify_quality( + const gps_pps_quality_metrics_t* metrics) +{ + if(!gps_pps_metrics_finite(metrics) || + !metrics->solution_valid || + (metrics->used_satellites < 4u) || + (metrics->tdop < 0.0) || + (metrics->gdop < 0.0) || + (metrics->clock_bias_sigma_m < 0.0)) + { + return GPS_PPS_PROFILE_HOLDOVER; + } + + bool residual_ok = + metrics->residual_test_valid ? + (isfinite(metrics->chi_square_ratio) && + (metrics->chi_square_ratio <= 1.0)) : + (metrics->used_satellites == 4u); + bool robust = + residual_ok && + (metrics->tdop <= 8.0) && + (metrics->gdop <= 12.0) && + (metrics->clock_bias_sigma_m <= 100.0) && + (metrics->mean_cn0_dbhz >= 22.0) && + (metrics->minimum_cn0_dbhz >= 14.0) && + (!metrics->velocity_valid || + (metrics->velocity_residual_rms_mps <= 3.0)); + if(!robust) + { + return GPS_PPS_PROFILE_HOLDOVER; + } + + bool precision = + (metrics->used_satellites >= 6u) && + metrics->residual_test_valid && + !metrics->fde_applied && + (metrics->tdop <= 3.0) && + (metrics->gdop <= 6.0) && + (metrics->clock_bias_sigma_m <= 35.0) && + (metrics->mean_cn0_dbhz >= 30.0) && + (metrics->minimum_cn0_dbhz >= 20.0) && + (metrics->chi_square_ratio <= 0.75) && + metrics->velocity_valid && + (metrics->velocity_residual_rms_mps <= 1.0); + return precision ? GPS_PPS_PROFILE_PRECISION : GPS_PPS_PROFILE_ROBUST; +} + +void gps_pps_quality_selector_init(gps_pps_quality_selector_t* selector) +{ + if(selector != NULL) + { + memset(selector, 0, sizeof(*selector)); + selector->profile = GPS_PPS_PROFILE_HOLDOVER; + selector->candidate = GPS_PPS_PROFILE_HOLDOVER; + } +} + +gps_pps_profile_t gps_pps_quality_selector_update( + gps_pps_quality_selector_t* selector, + const gps_pps_quality_metrics_t* metrics, + bool* profile_changed_out) +{ + if(profile_changed_out != NULL) + { + *profile_changed_out = false; + } + if(selector == NULL) + { + return GPS_PPS_PROFILE_HOLDOVER; + } + + gps_pps_profile_t target = + gps_pps_classify_quality(metrics); + + if(target == selector->profile) + { + selector->candidate = target; + selector->candidate_updates = 0u; + return selector->profile; + } + if(target != selector->candidate) + { + selector->candidate = target; + selector->candidate_updates = 1u; + } + else if(selector->candidate_updates != UINT_MAX) + { + selector->candidate_updates++; + } + + unsigned int required_updates = + (target > selector->profile) ? + GPS_PPS_PROFILE_UPGRADE_UPDATES : + GPS_PPS_PROFILE_DEGRADE_UPDATES; + if(selector->candidate_updates >= required_updates) + { + selector->profile = target; + selector->candidate_updates = 0u; + if(profile_changed_out != NULL) + { + *profile_changed_out = true; + } + } + return selector->profile; +} + +gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile) +{ + switch(profile) + { + case GPS_PPS_PROFILE_ACQUIRE: + return (gps_pps_loop_profile_t){ + .natural_frequency_hz = 0.120, + .damping = 0.9, + .max_slew_ppm = 20.0, + .lock_acquire_ticks = 8u, + .lock_release_ticks = 128u, + .lock_acquire_updates = 5u, + .software_bias_gain = 1.0 / 16.0, + .software_settle_updates = 12u + }; + case GPS_PPS_PROFILE_PRECISION: + return (gps_pps_loop_profile_t){ + .natural_frequency_hz = 0.035, + .damping = 0.9, + .max_slew_ppm = 20.0, + .lock_acquire_ticks = 8u, + .lock_release_ticks = 128u, + .lock_acquire_updates = 5u, + .software_bias_gain = 1.0 / 32.0, + .software_settle_updates = 12u + }; + case GPS_PPS_PROFILE_ROBUST: + return (gps_pps_loop_profile_t){ + .natural_frequency_hz = 0.012, + .damping = 1.0, + .max_slew_ppm = 20.0, + .lock_acquire_ticks = 8u, + .lock_release_ticks = 128u, + .lock_acquire_updates = 8u, + .software_bias_gain = 1.0 / 32.0, + .software_settle_updates = 16u + }; + case GPS_PPS_PROFILE_HOLDOVER: + default: + return (gps_pps_loop_profile_t){ + .natural_frequency_hz = 0.0, + .damping = 1.0, + .max_slew_ppm = 20.0, + .lock_acquire_ticks = 8u, + .lock_release_ticks = 128u, + .lock_acquire_updates = 8u, + .software_bias_gain = 1.0 / 32.0, + .software_settle_updates = 16u + }; + } +} + +const char* gps_pps_profile_name(gps_pps_profile_t profile) +{ + switch(profile) + { + case GPS_PPS_PROFILE_ACQUIRE: + return "acquire"; + case GPS_PPS_PROFILE_PRECISION: + return "precision"; + case GPS_PPS_PROFILE_ROBUST: + return "robust"; + case GPS_PPS_PROFILE_HOLDOVER: + default: + return "holdover"; + } +} diff --git a/soc/software/gps_system/src/gps/gps_pps_time_filter.c b/soc/software/gps_system/src/gps/gps_pps_time_filter.c new file mode 100644 index 0000000..0333ad0 --- /dev/null +++ b/soc/software/gps_system/src/gps/gps_pps_time_filter.c @@ -0,0 +1,309 @@ +#include "gps/gps_pps_time_filter.h" + +#include +#include +#include + +/* + * Board data showed hundreds of nanoseconds of epoch-to-epoch pseudorange + * clock-bias noise even with seven satellites, while Doppler clock drift was + * stable. A 1/32 residual gain gives roughly a 6.4 s time constant at the + * nominal 5 Hz solve rate. The innovation clamp limits one accepted WLS epoch + * to less than 32 ns of reference movement. + */ +#define GPS_PPS_FILTER_DRIFT_GAIN (1.0 / 4.0) +#define GPS_PPS_FILTER_FALLBACK_DRIFT_GAIN (1.0 / 16.0) +#define GPS_PPS_FILTER_MAX_DRIFT_MPS 10000.0 +#define GPS_PPS_FILTER_MAX_TDOP 8.0 +#define GPS_PPS_FILTER_MIN_INTERVAL_SEC 0.040 +#define GPS_PPS_FILTER_MAX_PREDICTION_GAP_SEC 30.000 +#define GPS_PPS_FILTER_INNOVATION_CLAMP_M 300.0 +#define GPS_PPS_FILTER_HARD_OUTLIER_M 1500.0 +#define GPS_PPS_SPEED_OF_LIGHT_MPS 299792458.0 + +static double clamp_double(double value, double limit) +{ + if(value > limit) + { + return limit; + } + if(value < -limit) + { + return -limit; + } + return value; +} + +void gps_pps_time_filter_init(gps_pps_time_filter_t* filter) +{ + if(filter != NULL) + { + memset(filter, 0, sizeof(*filter)); + } +} + +bool gps_pps_select_reference( + const unsigned int satellite_ids[], + const double capture_times[], + unsigned int count, + bool preferred_valid, + unsigned int preferred_id, + unsigned int* reference_index_out) +{ + if((satellite_ids == NULL) || (capture_times == NULL) || + (reference_index_out == NULL) || (count == 0u)) + { + return false; + } + + unsigned int earliest_index = 0u; + if(!isfinite(capture_times[0])) + { + return false; + } + for(unsigned int index = 0u; index < count; index++) + { + if(!isfinite(capture_times[index])) + { + return false; + } + if(preferred_valid && (satellite_ids[index] == preferred_id)) + { + *reference_index_out = index; + return true; + } + if(capture_times[index] < capture_times[earliest_index]) + { + earliest_index = index; + } + } + + *reference_index_out = earliest_index; + return true; +} + +bool gps_pps_reference_bias_drift( + const double receiver_ecef_m[3], + const double receiver_velocity_ecef_mps[3], + const double reference_satellite_ecef_m[3], + const double reference_satellite_velocity_ecef_mps[3], + double reference_satellite_clock_drift_sps, + double* clock_bias_drift_mps_out) +{ + if((receiver_ecef_m == NULL) || + (receiver_velocity_ecef_mps == NULL) || + (reference_satellite_ecef_m == NULL) || + (reference_satellite_velocity_ecef_mps == NULL) || + (clock_bias_drift_mps_out == NULL) || + !isfinite(reference_satellite_clock_drift_sps)) + { + return false; + } + + double delta_m[3]; + double range_squared_m2 = 0.0; + for(unsigned int component = 0u; component < 3u; component++) + { + if(!isfinite(receiver_ecef_m[component]) || + !isfinite(receiver_velocity_ecef_mps[component]) || + !isfinite(reference_satellite_ecef_m[component]) || + !isfinite(reference_satellite_velocity_ecef_mps[component])) + { + return false; + } + delta_m[component] = + reference_satellite_ecef_m[component] - + receiver_ecef_m[component]; + range_squared_m2 += delta_m[component] * delta_m[component]; + } + double range_m = sqrt(range_squared_m2); + if(!isfinite(range_m) || (range_m < 1.0)) + { + return false; + } + + double geometric_range_rate_mps = 0.0; + for(unsigned int component = 0u; component < 3u; component++) + { + double relative_velocity_mps = + reference_satellite_velocity_ecef_mps[component] - + receiver_velocity_ecef_mps[component]; + geometric_range_rate_mps += + (delta_m[component] / range_m) * + relative_velocity_mps; + } + /* + * capture_i - min(capture) removes the receiver clock before position + * WLS. The remaining bias datum is -range(reference), with the broadcast + * satellite-clock correction added to the observation. + */ + double bias_drift_mps = + -geometric_range_rate_mps + + GPS_PPS_SPEED_OF_LIGHT_MPS * + reference_satellite_clock_drift_sps; + if(!isfinite(bias_drift_mps)) + { + return false; + } + *clock_bias_drift_mps_out = bias_drift_mps; + return true; +} + +static gps_pps_time_filter_result_t gps_pps_time_filter_reanchor( + gps_pps_time_filter_t* filter, + uint64_t epoch_counter, + double measured_clock_bias_m, + bool clock_drift_valid, + double measured_clock_drift_mps, + double* filtered_clock_bias_m_out, + double* innovation_m_out) +{ + filter->epoch_counter = epoch_counter; + filter->drift_anchor_counter = epoch_counter; + filter->clock_bias_m = measured_clock_bias_m; + filter->clock_drift_mps = + clock_drift_valid ? measured_clock_drift_mps : 0.0; + filter->previous_measurement_m = measured_clock_bias_m; + filter->drift_anchor_measurement_m = measured_clock_bias_m; + filter->accepted_updates = 1u; + filter->initialized = true; + filter->drift_initialized = clock_drift_valid; + *filtered_clock_bias_m_out = measured_clock_bias_m; + *innovation_m_out = 0.0; + return GPS_PPS_TIME_FILTER_COARSE; +} + +gps_pps_time_filter_result_t gps_pps_time_filter_update( + gps_pps_time_filter_t* filter, + uint64_t epoch_counter, + double counter_frequency_hz, + double measured_clock_bias_m, + bool clock_drift_valid, + double measured_clock_drift_mps, + double tdop, + double bias_gain, + unsigned int settle_updates, + double* filtered_clock_bias_m_out, + double* innovation_m_out) +{ + if((filter == NULL) || (filtered_clock_bias_m_out == NULL) || + (innovation_m_out == NULL) || + !isfinite(counter_frequency_hz) || (counter_frequency_hz <= 0.0) || + !isfinite(measured_clock_bias_m) || + !isfinite(tdop) || (tdop < 0.0) || (tdop > GPS_PPS_FILTER_MAX_TDOP) || + !isfinite(bias_gain) || (bias_gain <= 0.0) || (bias_gain > 1.0) || + (settle_updates == 0u) || + (clock_drift_valid && !isfinite(measured_clock_drift_mps))) + { + return GPS_PPS_TIME_FILTER_REJECTED; + } + + if(!filter->initialized) + { + return gps_pps_time_filter_reanchor( + filter, epoch_counter, measured_clock_bias_m, + clock_drift_valid, measured_clock_drift_mps, + filtered_clock_bias_m_out, innovation_m_out); + } + + if(epoch_counter <= filter->epoch_counter) + { + return GPS_PPS_TIME_FILTER_REJECTED; + } + uint64_t elapsed_ticks = epoch_counter - filter->epoch_counter; + double elapsed_sec = (double)elapsed_ticks / counter_frequency_hz; + if(!isfinite(elapsed_sec) || + (elapsed_sec < GPS_PPS_FILTER_MIN_INTERVAL_SEC)) + { + return GPS_PPS_TIME_FILTER_REJECTED; + } + /* + * The independent two-second reference watchdog suppresses PPS validity + * during a missing-WLS interval. Do not discard the Doppler prediction + * or force a phase-stepping coarse commit for the shorter scheduling gaps + * observed on the board. Reference-satellite changes are reset by the + * caller; only a genuinely long same-datum gap is re-anchored here. + */ + if(elapsed_sec > GPS_PPS_FILTER_MAX_PREDICTION_GAP_SEC) + { + return gps_pps_time_filter_reanchor( + filter, epoch_counter, measured_clock_bias_m, + clock_drift_valid, measured_clock_drift_mps, + filtered_clock_bias_m_out, innovation_m_out); + } + + double measurement_delta_m = + measured_clock_bias_m - filter->previous_measurement_m; + if(!clock_drift_valid && + (fabs(measurement_delta_m) > + GPS_PPS_FILTER_MAX_DRIFT_MPS * elapsed_sec + + GPS_PPS_FILTER_HARD_OUTLIER_M)) + { + return GPS_PPS_TIME_FILTER_REJECTED; + } + double anchor_elapsed_sec = + (double)(epoch_counter - filter->drift_anchor_counter) / + counter_frequency_hz; + double fallback_drift_mps = clamp_double( + (anchor_elapsed_sec > 0.0) ? + ((measured_clock_bias_m - + filter->drift_anchor_measurement_m) / + anchor_elapsed_sec) : + (measurement_delta_m / elapsed_sec), + GPS_PPS_FILTER_MAX_DRIFT_MPS); + if(!filter->drift_initialized) + { + /* This happens only while PPS is still coarse. It prevents the + * hundreds-of-metres/second receiver-clock ramp from being mistaken + * for timing phase when Doppler velocity is temporarily absent. */ + filter->clock_drift_mps = fallback_drift_mps; + filter->drift_initialized = true; + } + + double predicted_clock_bias_m = + filter->clock_bias_m + filter->clock_drift_mps * elapsed_sec; + double innovation_m = measured_clock_bias_m - predicted_clock_bias_m; + if(!isfinite(predicted_clock_bias_m) || !isfinite(innovation_m) || + (fabs(innovation_m) > GPS_PPS_FILTER_HARD_OUTLIER_M)) + { + return GPS_PPS_TIME_FILTER_REJECTED; + } + + filter->clock_bias_m = + predicted_clock_bias_m + + bias_gain * + clamp_double(innovation_m, GPS_PPS_FILTER_INNOVATION_CLAMP_M); + if(clock_drift_valid) + { + filter->clock_drift_mps += + GPS_PPS_FILTER_DRIFT_GAIN * + (measured_clock_drift_mps - filter->clock_drift_mps); + } + else + { + if(filter->accepted_updates < settle_updates) + { + /* While PPS output is deliberately invalid/coarse, use the slope + * from the re-anchor point so a single noisy first difference + * cannot leave a long clock-ramp transient. */ + filter->clock_drift_mps = fallback_drift_mps; + } + else + { + filter->clock_drift_mps += + GPS_PPS_FILTER_FALLBACK_DRIFT_GAIN * + (fallback_drift_mps - filter->clock_drift_mps); + } + } + filter->previous_measurement_m = measured_clock_bias_m; + filter->epoch_counter = epoch_counter; + if(filter->accepted_updates != UINT32_MAX) + { + filter->accepted_updates++; + } + + *filtered_clock_bias_m_out = filter->clock_bias_m; + *innovation_m_out = innovation_m; + return (filter->accepted_updates < settle_updates) ? + GPS_PPS_TIME_FILTER_COARSE : GPS_PPS_TIME_FILTER_TRACKING; +} diff --git a/soc/software/gps_system/src/gps/gps_time_anchor.c b/soc/software/gps_system/src/gps/gps_time_anchor.c index 53125dd..e52a8b9 100644 --- a/soc/software/gps_system/src/gps/gps_time_anchor.c +++ b/soc/software/gps_system/src/gps/gps_time_anchor.c @@ -109,6 +109,109 @@ bool gps_time_from_sample_anchor(int anchor_week, return true; } +bool gps_time_from_periodic_sample_anchor( + int anchor_week, + double anchor_tow_sec, + uint32_t anchor_raw_sample, + double anchor_corrected_sample, + uint32_t event_raw_sample, + double event_corrected_sample, + double samples_per_second, + double max_age_sec, + double event_period_sec, + double max_snap_error_sec, + int* event_week_out, + double* event_tow_sec_out, + double* elapsed_sec_out) +{ + if((event_week_out == NULL) || (event_tow_sec_out == NULL) || + !(event_period_sec > 0.0) || !(max_snap_error_sec >= 0.0) || + !isfinite(event_period_sec) || !isfinite(max_snap_error_sec)) + { + return false; + } + + int sample_week = 0; + double sample_tow_sec = 0.0; + double sample_elapsed_sec = 0.0; + if(!gps_time_from_sample_anchor( + anchor_week, anchor_tow_sec, + anchor_raw_sample, anchor_corrected_sample, + event_raw_sample, event_corrected_sample, + samples_per_second, max_age_sec, + &sample_week, &sample_tow_sec, &sample_elapsed_sec)) + { + return false; + } + (void)sample_week; + (void)sample_tow_sec; + + double event_index = round(sample_elapsed_sec / event_period_sec); + double periodic_elapsed_sec = event_index * event_period_sec; + if(!isfinite(event_index) || !isfinite(periodic_elapsed_sec) || + (event_index < 0.0) || + (fabs(periodic_elapsed_sec - sample_elapsed_sec) > + max_snap_error_sec)) + { + return false; + } + + int event_week = anchor_week; + double event_tow_sec = gps_time_normalize_week( + anchor_tow_sec + periodic_elapsed_sec, &event_week); + *event_week_out = event_week; + *event_tow_sec_out = event_tow_sec; + if(elapsed_sec_out != NULL) + { + *elapsed_sec_out = periodic_elapsed_sec; + } + return true; +} + +bool gps_time_correct_capture_counter( + uint64_t capture_counter, + double fractional_sample_delay, + double samples_per_second, + double counter_frequency_hz, + uint64_t* corrected_counter_out, + double* applied_correction_sec_out) +{ + if((corrected_counter_out == NULL) || + !isfinite(fractional_sample_delay) || + !isfinite(samples_per_second) || + !isfinite(counter_frequency_hz) || + (fractional_sample_delay < 0.0) || + (fractional_sample_delay > 1.0 + 1.0e-6) || + !(samples_per_second > 0.0) || + !(counter_frequency_hz > 0.0)) + { + return false; + } + + double correction_ticks_exact = + fractional_sample_delay * + (counter_frequency_hz / samples_per_second); + if(!isfinite(correction_ticks_exact) || + (correction_ticks_exact < 0.0) || + (correction_ticks_exact > (double)INT64_MAX)) + { + return false; + } + + uint64_t correction_ticks = (uint64_t)llround(correction_ticks_exact); + if(correction_ticks > capture_counter) + { + return false; + } + *corrected_counter_out = capture_counter - correction_ticks; + if(applied_correction_sec_out != NULL) + { + *applied_correction_sec_out = + (double)correction_ticks / counter_frequency_hz; + } + return true; +} + bool gps_time_anchors_consistent(int first_week, double first_tow_sec, uint32_t first_raw_sample, diff --git a/soc/software/gps_system/tests/Makefile b/soc/software/gps_system/tests/Makefile index eafc789..dd1c62d 100644 --- a/soc/software/gps_system/tests/Makefile +++ b/soc/software/gps_system/tests/Makefile @@ -8,6 +8,9 @@ TARGETS := test_imu_calibration test_gps_nav_epoch \ test_gps_ephemeris_cache test_gps_receiver_hardware_sim TARGETS += test_gps_tracking_measurement TARGETS += test_gps_satellite_velocity +TARGETS += test_gps_pps_time_filter +TARGETS += test_gps_pps_quality +TARGETS += test_gps_pps_adaptive_loop .PHONY: all run sanitize clean @@ -50,6 +53,19 @@ test_gps_satellite_velocity: test_gps_satellite_velocity.c \ -Wno-unused-variable -Wno-sign-compare \ -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@ +test_gps_pps_time_filter: test_gps_pps_time_filter.c \ + $(ROOT)/src/gps/gps_pps_time_filter.c + $(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@ + +test_gps_pps_quality: test_gps_pps_quality.c \ + $(ROOT)/src/gps/gps_pps_quality.c + $(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@ + +test_gps_pps_adaptive_loop: test_gps_pps_adaptive_loop.c \ + $(ROOT)/src/gps/gps_pps_time_filter.c \ + $(ROOT)/src/gps/gps_pps_quality.c + $(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@ + run: $(TARGETS) ./test_imu_calibration ./test_gps_nav_epoch @@ -61,6 +77,9 @@ run: $(TARGETS) ./test_gps_receiver_hardware_sim ./test_gps_tracking_measurement ./test_gps_satellite_velocity + ./test_gps_pps_time_filter + ./test_gps_pps_quality + ./test_gps_pps_adaptive_loop sanitize: $(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \ @@ -77,6 +96,13 @@ sanitize: -o test_gps_nav_epoch_coalescing_san ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ UBSAN_OPTIONS=halt_on_error=1 ./test_gps_nav_epoch_coalescing_san + $(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \ + -fsanitize=address,undefined -fno-omit-frame-pointer \ + -I$(ROOT)/header test_gps_time_anchor.c \ + $(ROOT)/src/gps/gps_time_anchor.c -lm \ + -o test_gps_time_anchor_san + ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ + UBSAN_OPTIONS=halt_on_error=1 ./test_gps_time_anchor_san $(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \ -fsanitize=address,undefined -fno-omit-frame-pointer \ -I$(ROOT)/header test_gps_acq_manager.c \ @@ -122,11 +148,36 @@ sanitize: ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ UBSAN_OPTIONS=halt_on_error=1 \ ./test_gps_satellite_velocity_san + $(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \ + -fsanitize=address,undefined -fno-omit-frame-pointer \ + -I$(ROOT)/header test_gps_pps_time_filter.c \ + $(ROOT)/src/gps/gps_pps_time_filter.c -lm \ + -o test_gps_pps_time_filter_san + ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ + UBSAN_OPTIONS=halt_on_error=1 ./test_gps_pps_time_filter_san + $(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \ + -fsanitize=address,undefined -fno-omit-frame-pointer \ + -I$(ROOT)/header test_gps_pps_quality.c \ + $(ROOT)/src/gps/gps_pps_quality.c -lm \ + -o test_gps_pps_quality_san + ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ + UBSAN_OPTIONS=halt_on_error=1 ./test_gps_pps_quality_san + $(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \ + -fsanitize=address,undefined -fno-omit-frame-pointer \ + -I$(ROOT)/header test_gps_pps_adaptive_loop.c \ + $(ROOT)/src/gps/gps_pps_time_filter.c \ + $(ROOT)/src/gps/gps_pps_quality.c -lm \ + -o test_gps_pps_adaptive_loop_san + ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ + UBSAN_OPTIONS=halt_on_error=1 ./test_gps_pps_adaptive_loop_san clean: rm -f $(TARGETS) test_gps_nav_epoch_coalescing_repro \ test_gps_nav_epoch_san test_gps_nav_epoch_coalescing_san \ + test_gps_time_anchor_san \ test_gps_acq_manager_san test_gps_ephemeris_cache_san \ test_gps_receiver_hardware_sim_san \ test_gps_tracking_measurement_san \ - test_gps_satellite_velocity_san + test_gps_satellite_velocity_san \ + test_gps_pps_time_filter_san test_gps_pps_quality_san \ + test_gps_pps_adaptive_loop_san diff --git a/soc/software/gps_system/tests/test_gps_pps_adaptive_loop.c b/soc/software/gps_system/tests/test_gps_pps_adaptive_loop.c new file mode 100644 index 0000000..2dfda0e --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_pps_adaptive_loop.c @@ -0,0 +1,542 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "gps/gps_pps_quality.h" +#include "gps/gps_pps_time_filter.h" + +#define TRACKING_HZ 125000000.0 +#define UPDATE_PERIOD_SEC 0.2 +#define SPEED_OF_LIGHT_MPS 299792458.0 +#define Q32_SCALE 4294967296.0 +#define Q46_SCALE 70368744177664.0 + +typedef struct { + int64_t phase_q32; + int64_t integrator_q32; + int64_t rate_q32; + bool aligned; + bool locked; + unsigned int good_updates; +} synthetic_rtl_t; + +typedef struct { + uint32_t state; +} synthetic_rng_t; + +typedef enum { + SYNTHETIC_STEADY = 0, + SYNTHETIC_STARTUP_NOISY_THEN_CLEAN, + SYNTHETIC_NOISE_BURSTS, + SYNTHETIC_SATELLITE_LOSS, + SYNTHETIC_DOPPLER_LOSS, + SYNTHETIC_GEOMETRY_DEGRADE, + SYNTHETIC_REFERENCE_RANGE_RATE +} synthetic_pattern_t; + +static double synthetic_uniform(synthetic_rng_t* rng) +{ + rng->state = rng->state * 1664525u + 1013904223u; + return ((double)(rng->state >> 8) + 0.5) / 16777216.0; +} + +static double synthetic_normal(synthetic_rng_t* rng) +{ + double sum = 0.0; + for(unsigned int sample = 0u; sample < 12u; sample++) + { + sum += synthetic_uniform(rng); + } + return sum - 6.0; +} + +static int64_t clamp_i128(__int128 value, int64_t limit) +{ + if(value > limit) + { + return limit; + } + if(value < -(__int128)limit) + { + return -limit; + } + return (int64_t)value; +} + +static int64_t seconds_to_phase_q32(double seconds) +{ + return (int64_t)llround(seconds * TRACKING_HZ * Q32_SCALE); +} + +static double phase_q32_to_seconds(int64_t phase_q32) +{ + return (double)phase_q32 / (TRACKING_HZ * Q32_SCALE); +} + +static void synthetic_rtl_advance(synthetic_rtl_t* rtl, double oscillator_ppm) +{ + int64_t raw_ticks = (int64_t)llround(UPDATE_PERIOD_SEC * TRACKING_HZ); + int64_t oscillator_q32 = (int64_t)llround( + oscillator_ppm * 1.0e-6 * Q32_SCALE); + rtl->phase_q32 += raw_ticks * (oscillator_q32 + rtl->rate_q32); +} + +static void profile_coefficients( + gps_pps_loop_profile_t profile, + int64_t* kp_q46, + int64_t* ki_q46, + int64_t* max_slew_q32) +{ + const double pi = 3.14159265358979323846; + double wn = 2.0 * pi * profile.natural_frequency_hz; + *kp_q46 = (int64_t)llround( + (2.0 * profile.damping * wn / TRACKING_HZ) * Q46_SCALE); + *ki_q46 = (int64_t)llround( + (wn * wn * UPDATE_PERIOD_SEC / TRACKING_HZ) * Q46_SCALE); + *max_slew_q32 = (int64_t)llround( + profile.max_slew_ppm * 1.0e-6 * Q32_SCALE); +} + +static void synthetic_rtl_coarse(synthetic_rtl_t* rtl, int64_t reference_q32) +{ + bool was_locked = rtl->locked; + memset(rtl, 0, sizeof(*rtl)); + rtl->phase_q32 = reference_q32; + rtl->aligned = true; + assert(!was_locked || !rtl->locked); +} + +static void synthetic_rtl_commit( + synthetic_rtl_t* rtl, + int64_t reference_q32, + gps_pps_loop_profile_t profile) +{ + int64_t raw_phase_q32 = reference_q32 - rtl->phase_q32; + + int64_t kp_q46; + int64_t ki_q46; + int64_t max_slew_q32; + profile_coefficients(profile, &kp_q46, &ki_q46, &max_slew_q32); + int64_t proportional_q32 = + (int64_t)(((__int128)raw_phase_q32 * kp_q46) >> 46); + __int128 integrator_candidate = + (__int128)rtl->integrator_q32 + + (((__int128)raw_phase_q32 * ki_q46) >> 46); + rtl->integrator_q32 = + clamp_i128(integrator_candidate, max_slew_q32); + rtl->rate_q32 = clamp_i128( + (__int128)rtl->integrator_q32 + proportional_q32, + max_slew_q32); + + int64_t acquire_q32 = + (int64_t)profile.lock_acquire_ticks << 32; + int64_t release_q32 = + (int64_t)profile.lock_release_ticks << 32; + if(llabs(raw_phase_q32) > release_q32) + { + rtl->locked = false; + rtl->good_updates = 0u; + } + else if(!rtl->locked) + { + if(llabs(raw_phase_q32) <= acquire_q32) + { + rtl->good_updates++; + if(rtl->good_updates >= profile.lock_acquire_updates) + { + rtl->locked = true; + rtl->good_updates = 0u; + } + } + else + { + rtl->good_updates = 0u; + } + } +} + +static gps_pps_quality_metrics_t precision_quality(void) +{ + return (gps_pps_quality_metrics_t){ + .tdop = 1.7, + .gdop = 3.2, + .clock_bias_sigma_m = 18.0, + .mean_cn0_dbhz = 38.0, + .minimum_cn0_dbhz = 27.0, + .chi_square_ratio = 0.35, + .velocity_residual_rms_mps = 0.25, + .used_satellites = 7u, + .solution_valid = true, + .residual_test_valid = true, + .velocity_valid = true, + .fde_applied = false + }; +} + +static gps_pps_quality_metrics_t robust_quality(bool velocity_valid) +{ + gps_pps_quality_metrics_t quality = { + .tdop = 5.0, + .gdop = 8.0, + .clock_bias_sigma_m = 65.0, + .mean_cn0_dbhz = 27.0, + .minimum_cn0_dbhz = 18.0, + .chi_square_ratio = 0.85, + .velocity_residual_rms_mps = velocity_valid ? 1.8 : 0.0, + .used_satellites = 5u, + .solution_valid = true, + .residual_test_valid = true, + .velocity_valid = velocity_valid, + .fde_applied = false + }; + return quality; +} + +static void run_noisy_scenario( + unsigned int seed, + bool with_gap, + double oscillator_ppm, + double noise_sigma_ns, + bool clock_drift_valid, + bool use_robust_quality, + bool hard_outliers, + unsigned int update_count, + synthetic_pattern_t pattern, + double* first_lock_sec_out, + double* one_second_jitter_rms_sec_out, + unsigned int* coarse_count_out, + unsigned int* rejected_count_out, + bool* locked_after_recovery_out) +{ + assert(update_count <= 1500u); + synthetic_rng_t rng = {.state = seed}; + gps_pps_quality_selector_t selector; + gps_pps_quality_selector_init(&selector); + gps_pps_time_filter_t filter; + gps_pps_time_filter_init(&filter); + synthetic_rtl_t rtl = {0}; + gps_pps_profile_t quality_profile = GPS_PPS_PROFILE_HOLDOVER; + double first_lock_sec = NAN; + unsigned int coarse_count = 0u; + unsigned int rejected_count = 0u; + double phase_history[1500] = {0.0}; + bool lock_history[1500] = {false}; + + for(unsigned int update = 0u; update < update_count; update++) + { + double time_sec = UPDATE_PERIOD_SEC * (double)update; + synthetic_rtl_advance(&rtl, oscillator_ppm); + phase_history[update] = phase_q32_to_seconds(rtl.phase_q32); + lock_history[update] = rtl.locked; + if(with_gap && (time_sec >= 35.0) && (time_sec < 50.0)) + { + /* The firmware reference watchdog deasserts PPS validity after + * two seconds without a successful commit. */ + if(time_sec >= 37.0) + { + lock_history[update] = false; + } + continue; + } + + bool sample_drift_valid = clock_drift_valid; + bool sample_robust_quality = use_robust_quality; + bool sample_solution_valid = true; + double sample_noise_sigma_ns = noise_sigma_ns; + double reference_range_rate_mps = 0.0; + switch(pattern) + { + case SYNTHETIC_STARTUP_NOISY_THEN_CLEAN: + sample_robust_quality = time_sec < 55.0; + sample_noise_sigma_ns = + (time_sec < 55.0) ? 1100.0 : 120.0; + break; + case SYNTHETIC_NOISE_BURSTS: + { + bool burst = + ((time_sec >= 55.0) && (time_sec < 78.0)) || + ((time_sec >= 135.0) && (time_sec < 151.0)) || + ((time_sec >= 205.0) && (time_sec < 214.0)); + sample_robust_quality = burst; + sample_noise_sigma_ns = burst ? 900.0 : 150.0; + break; + } + case SYNTHETIC_SATELLITE_LOSS: + sample_solution_valid = + !(((time_sec >= 55.0) && (time_sec < 78.0)) || + ((time_sec >= 145.0) && (time_sec < 151.0))); + break; + case SYNTHETIC_DOPPLER_LOSS: + sample_drift_valid = + !((time_sec >= 65.0) && (time_sec < 105.0)); + break; + case SYNTHETIC_GEOMETRY_DEGRADE: + sample_robust_quality = + (time_sec >= 60.0) && (time_sec < 145.0); + sample_noise_sigma_ns = + sample_robust_quality ? 650.0 : 200.0; + break; + case SYNTHETIC_REFERENCE_RANGE_RATE: + reference_range_rate_mps = 130.0; + break; + case SYNTHETIC_STEADY: + default: + break; + } + + bool changed = false; + gps_pps_quality_metrics_t quality = sample_robust_quality ? + robust_quality(sample_drift_valid) : precision_quality(); + quality.velocity_valid = sample_drift_valid; + if(!sample_solution_valid) + { + quality.solution_valid = false; + quality.used_satellites = 2u; + quality.velocity_valid = false; + } + quality_profile = gps_pps_quality_selector_update( + &selector, &quality, &changed); + if(quality_profile == GPS_PPS_PROFILE_HOLDOVER) + { + lock_history[update] = false; + continue; + } + gps_pps_profile_t loop_profile = + rtl.locked ? quality_profile : GPS_PPS_PROFILE_ACQUIRE; + gps_pps_loop_profile_t parameters = + gps_pps_loop_profile(loop_profile); + + double true_bias_m = + 1200.0 + + (-reference_range_rate_mps) * time_sec; + double noise_sec = + synthetic_normal(&rng) * sample_noise_sigma_ns * 1.0e-9; + if((update % 37u) == 0u) + { + noise_sec += + (synthetic_uniform(&rng) < 0.5 ? -1.0 : 1.0) * + (hard_outliers ? 6.0e-6 : 900.0e-9); + } + double measured_bias_m = + true_bias_m - noise_sec * SPEED_OF_LIGHT_MPS; + double filtered_bias_m = 0.0; + double innovation_m = 0.0; + gps_pps_time_filter_result_t result = gps_pps_time_filter_update( + &filter, + (uint64_t)llround(time_sec * TRACKING_HZ), + TRACKING_HZ, + measured_bias_m, + sample_drift_valid, + -reference_range_rate_mps + + synthetic_normal(&rng) * 0.25, + quality.tdop, + parameters.software_bias_gain, + parameters.software_settle_updates, + &filtered_bias_m, + &innovation_m); + if(result == GPS_PPS_TIME_FILTER_REJECTED) + { + rejected_count++; + continue; + } + assert(isfinite(innovation_m)); + int64_t reference_q32 = seconds_to_phase_q32( + -(filtered_bias_m - true_bias_m) / SPEED_OF_LIGHT_MPS); + if((result == GPS_PPS_TIME_FILTER_COARSE) || !rtl.aligned) + { + synthetic_rtl_coarse(&rtl, reference_q32); + coarse_count++; + } + else + { + synthetic_rtl_commit(&rtl, reference_q32, parameters); + } + if(rtl.locked && !isfinite(first_lock_sec)) + { + first_lock_sec = time_sec; + } + phase_history[update] = phase_q32_to_seconds(rtl.phase_q32); + lock_history[update] = rtl.locked; + } + + double jitter_square_sum = 0.0; + unsigned int jitter_count = 0u; + for(unsigned int update = (update_count / 2u) + 5u; + update < update_count; + update++) + { + if(lock_history[update] && lock_history[update - 5u]) + { + double difference = + phase_history[update] - phase_history[update - 5u]; + jitter_square_sum += difference * difference; + jitter_count++; + } + } + *first_lock_sec_out = first_lock_sec; + *one_second_jitter_rms_sec_out = + (jitter_count != 0u) ? + sqrt(jitter_square_sum / (double)jitter_count) : NAN; + *coarse_count_out = coarse_count; + *rejected_count_out = rejected_count; + *locked_after_recovery_out = lock_history[update_count - 1u]; +} + +static void test_noisy_fixed_point_loop(void) +{ + for(unsigned int seed = 1u; seed <= 20u; seed++) + { + double first_lock_sec; + double jitter_rms_sec; + unsigned int coarse_count; + unsigned int rejected_count; + bool locked_after_recovery; + run_noisy_scenario( + seed, false, 1.65, 320.0, true, false, false, 600u, + SYNTHETIC_STEADY, + &first_lock_sec, &jitter_rms_sec, &coarse_count, + &rejected_count, &locked_after_recovery); + assert(isfinite(first_lock_sec)); + assert(first_lock_sec < 15.0); + assert(jitter_rms_sec < 20.0e-9); + assert(coarse_count >= 10u); + assert(rejected_count == 0u); + assert(locked_after_recovery); + } +} + +static void test_missing_wls_holds_and_resumes_without_reanchor(void) +{ + double first_lock_sec; + double jitter_rms_sec; + unsigned int coarse_count; + unsigned int rejected_count; + bool locked_after_recovery; + run_noisy_scenario( + 31u, true, 1.65, 320.0, true, false, false, 600u, + SYNTHETIC_STEADY, + &first_lock_sec, &jitter_rms_sec, &coarse_count, + &rejected_count, &locked_after_recovery); + assert(first_lock_sec < 15.0); + /* Initial settling emits repeated coarse commits; the bounded 15-second + * gap must not add another reset of the already converged PI state. */ + assert(coarse_count == 11u); + assert(rejected_count == 0u); + assert(locked_after_recovery); + assert(jitter_rms_sec < 20.0e-9); +} + +static void test_embedded_corner_case_matrix(void) +{ + for(unsigned int scenario = 0u; scenario < 4u; scenario++) + { + double first_lock_sec; + double jitter_rms_sec; + unsigned int coarse_count; + unsigned int rejected_count; + bool locked_after_recovery; + bool no_doppler = scenario == 1u; + bool hard_outliers = scenario == 2u; + double oscillator_ppm = (scenario == 3u) ? 18.0 : 1.65; + double noise_sigma_ns = (scenario == 0u) ? 650.0 : 320.0; + run_noisy_scenario( + 100u + scenario, + false, + oscillator_ppm, + noise_sigma_ns, + !no_doppler, + (scenario == 0u) || no_doppler, + hard_outliers, + 1200u, + SYNTHETIC_STEADY, + &first_lock_sec, + &jitter_rms_sec, + &coarse_count, + &rejected_count, + &locked_after_recovery); + assert(isfinite(first_lock_sec)); + assert(first_lock_sec < 35.0); + assert(isfinite(jitter_rms_sec)); + assert(jitter_rms_sec < 20.0e-9); + assert(coarse_count >= 10u); + assert(hard_outliers ? (rejected_count > 0u) : + (rejected_count == 0u)); + assert(locked_after_recovery); + } + + double first_lock_sec; + double jitter_rms_sec; + unsigned int coarse_count; + unsigned int rejected_count; + bool locked_after_recovery; + run_noisy_scenario( + 200u, false, 25.0, 50.0, true, false, false, 600u, + SYNTHETIC_STEADY, + &first_lock_sec, &jitter_rms_sec, &coarse_count, + &rejected_count, &locked_after_recovery); + assert(!isfinite(first_lock_sec)); + assert(!isfinite(jitter_rms_sec)); + assert(!locked_after_recovery); +} + +static void test_time_varying_environment_matrix(void) +{ + const synthetic_pattern_t patterns[] = { + SYNTHETIC_STARTUP_NOISY_THEN_CLEAN, + SYNTHETIC_NOISE_BURSTS, + SYNTHETIC_SATELLITE_LOSS, + SYNTHETIC_DOPPLER_LOSS, + SYNTHETIC_GEOMETRY_DEGRADE, + SYNTHETIC_REFERENCE_RANGE_RATE + }; + for(unsigned int pattern_index = 0u; + pattern_index < (sizeof(patterns) / sizeof(patterns[0])); + pattern_index++) + { + for(unsigned int seed = 1u; seed <= 12u; seed++) + { + double first_lock_sec; + double jitter_rms_sec; + unsigned int coarse_count; + unsigned int rejected_count; + bool locked_after_recovery; + run_noisy_scenario( + 300u + 31u * pattern_index + seed, + false, + (patterns[pattern_index] == SYNTHETIC_NOISE_BURSTS) ? + -2.5 : 1.65, + 320.0, + true, + false, + false, + 1500u, + patterns[pattern_index], + &first_lock_sec, + &jitter_rms_sec, + &coarse_count, + &rejected_count, + &locked_after_recovery); + assert(isfinite(first_lock_sec)); + assert(first_lock_sec < 25.0); + assert(isfinite(jitter_rms_sec)); + assert(jitter_rms_sec < 20.0e-9); + assert(coarse_count >= 10u); + assert(rejected_count == 0u); + assert(locked_after_recovery); + } + } +} + +int main(void) +{ + test_noisy_fixed_point_loop(); + test_missing_wls_holds_and_resumes_without_reanchor(); + test_embedded_corner_case_matrix(); + test_time_varying_environment_matrix(); + puts("gps_pps_adaptive_loop tests passed"); + return 0; +} diff --git a/soc/software/gps_system/tests/test_gps_pps_quality.c b/soc/software/gps_system/tests/test_gps_pps_quality.c new file mode 100644 index 0000000..8a5314a --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_pps_quality.c @@ -0,0 +1,173 @@ +#include +#include +#include + +#include "gps/gps_pps_quality.h" + +static gps_pps_quality_metrics_t precision_metrics(void) +{ + return (gps_pps_quality_metrics_t){ + .tdop = 1.7, + .gdop = 3.2, + .clock_bias_sigma_m = 18.0, + .mean_cn0_dbhz = 38.0, + .minimum_cn0_dbhz = 29.0, + .chi_square_ratio = 0.35, + .velocity_residual_rms_mps = 0.25, + .used_satellites = 7u, + .solution_valid = true, + .residual_test_valid = true, + .velocity_valid = true, + .fde_applied = false + }; +} + +static void test_hysteretic_profile_transitions(void) +{ + gps_pps_quality_selector_t selector; + gps_pps_quality_selector_init(&selector); + gps_pps_quality_metrics_t metrics = precision_metrics(); + bool changed = false; + + for(unsigned int update = 0u; update < 7u; update++) + { + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER); + assert(!changed); + } + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION); + assert(changed); + + /* One DOP/CN0 excursion must not chatter the active loop profile. */ + metrics.tdop = 4.5; + metrics.gdop = 7.0; + metrics.mean_cn0_dbhz = 27.0; + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION); + assert(!changed); + metrics = precision_metrics(); + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION); + + /* Sustained FDE operation degrades to the robust profile. */ + metrics.fde_applied = true; + for(unsigned int update = 0u; update < 2u; update++) + { + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION); + } + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_ROBUST); + assert(changed); + + /* One invalid WLS epoch is skipped; sustained invalidity holds over. */ + metrics.solution_valid = false; + for(unsigned int update = 0u; update < 2u; update++) + { + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_ROBUST); + assert(!changed); + } + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER); + assert(changed); +} + +static void test_alternating_quality_does_not_upgrade(void) +{ + gps_pps_quality_selector_t selector; + gps_pps_quality_selector_init(&selector); + gps_pps_quality_metrics_t good = precision_metrics(); + gps_pps_quality_metrics_t poor = good; + poor.tdop = 9.0; + + for(unsigned int update = 0u; update < 40u; update++) + { + const gps_pps_quality_metrics_t* metrics = + (update & 1u) ? &poor : &good; + bool changed = false; + assert(gps_pps_quality_selector_update( + &selector, metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER); + assert(!changed); + } +} + +static void test_profile_parameters_are_ordered(void) +{ + gps_pps_loop_profile_t robust = + gps_pps_loop_profile(GPS_PPS_PROFILE_ROBUST); + gps_pps_loop_profile_t precision = + gps_pps_loop_profile(GPS_PPS_PROFILE_PRECISION); + gps_pps_loop_profile_t acquire = + gps_pps_loop_profile(GPS_PPS_PROFILE_ACQUIRE); + assert(robust.natural_frequency_hz < precision.natural_frequency_hz); + assert(precision.natural_frequency_hz < acquire.natural_frequency_hz); + assert(robust.lock_acquire_updates > precision.lock_acquire_updates); + assert(robust.lock_release_ticks >= precision.lock_release_ticks); +} + +static void test_quality_boundaries_and_recovery(void) +{ + gps_pps_quality_selector_t selector; + gps_pps_quality_selector_init(&selector); + gps_pps_quality_metrics_t metrics = precision_metrics(); + bool changed = false; + + /* Exactly-on-threshold values remain usable in robust mode. */ + metrics.used_satellites = 5u; + metrics.tdop = 8.0; + metrics.gdop = 12.0; + metrics.clock_bias_sigma_m = 100.0; + metrics.mean_cn0_dbhz = 22.0; + metrics.minimum_cn0_dbhz = 14.0; + metrics.chi_square_ratio = 1.0; + metrics.velocity_residual_rms_mps = 3.0; + for(unsigned int update = 0u; update < 8u; update++) + { + gps_pps_quality_selector_update(&selector, &metrics, &changed); + } + assert(selector.profile == GPS_PPS_PROFILE_ROBUST); + + /* Non-finite metrics and loss below four satellites use fast hysteresis. */ + metrics.tdop = NAN; + for(unsigned int update = 0u; update < 2u; update++) + { + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_ROBUST); + } + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER); + assert(changed); + + /* A single good epoch is not enough to leave holdover. */ + metrics = precision_metrics(); + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER); + metrics.used_satellites = 3u; + for(unsigned int update = 0u; update < 3u; update++) + { + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER); + } + + /* Recovery cannot expose PPS until eight consecutive good epochs. */ + metrics = precision_metrics(); + for(unsigned int update = 0u; update < 7u; update++) + { + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER); + } + assert(gps_pps_quality_selector_update( + &selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION); +} + +int main(void) +{ + test_hysteretic_profile_transitions(); + test_alternating_quality_does_not_upgrade(); + test_profile_parameters_are_ordered(); + test_quality_boundaries_and_recovery(); + puts("gps_pps_quality tests passed"); + return 0; +} diff --git a/soc/software/gps_system/tests/test_gps_pps_time_filter.c b/soc/software/gps_system/tests/test_gps_pps_time_filter.c new file mode 100644 index 0000000..d3577ba --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_pps_time_filter.c @@ -0,0 +1,215 @@ +#include +#include +#include + +#include "gps/gps_pps_time_filter.h" + +#define COUNTER_HZ 125000000.0 + +static void test_drift_prediction_rejects_bias_noise(void) +{ + static const double noise_m[] = { + 0.0, 90.0, -120.0, 70.0, -45.0, 130.0, -80.0, 55.0, + -105.0, 35.0, 85.0, -65.0, 115.0, -95.0, 50.0, -30.0 + }; + gps_pps_time_filter_t filter; + gps_pps_time_filter_init(&filter); + + double squared_error_sum = 0.0; + unsigned int tracked = 0u; + for(unsigned int update = 0u; update < 160u; update++) + { + double time_sec = 0.2 * (double)update; + double true_bias_m = 1200.0 + 475.0 * time_sec; + double measured_bias_m = + true_bias_m + noise_m[update % (sizeof(noise_m) / sizeof(noise_m[0]))]; + double filtered_bias_m = 0.0; + double innovation_m = 0.0; + gps_pps_time_filter_result_t result = gps_pps_time_filter_update( + &filter, + (uint64_t)llround(time_sec * COUNTER_HZ), + COUNTER_HZ, + measured_bias_m, + true, + 475.0 + ((update & 1u) ? 0.3 : -0.3), + 1.7, + 1.0 / 32.0, + 16u, + &filtered_bias_m, + &innovation_m); + assert(result != GPS_PPS_TIME_FILTER_REJECTED); + assert(isfinite(innovation_m)); + if(result == GPS_PPS_TIME_FILTER_TRACKING) + { + double error_m = filtered_bias_m - true_bias_m; + squared_error_sum += error_m * error_m; + tracked++; + } + } + assert(tracked > 100u); + assert(sqrt(squared_error_sum / (double)tracked) < 12.0); +} + +static void test_quality_gap_and_outlier_handling(void) +{ + gps_pps_time_filter_t filter; + gps_pps_time_filter_init(&filter); + double filtered = 0.0; + double innovation = 0.0; + + assert(gps_pps_time_filter_update( + &filter, 0u, COUNTER_HZ, 100.0, true, 10.0, 1.0, 1.0 / 32.0, 1u, + &filtered, &innovation) == GPS_PPS_TIME_FILTER_COARSE); + assert(gps_pps_time_filter_update( + &filter, 25000000u, COUNTER_HZ, 102.0, true, 10.0, 12.0, 1.0 / 32.0, 1u, + &filtered, &innovation) == GPS_PPS_TIME_FILTER_REJECTED); + + uint64_t saved_epoch = filter.epoch_counter; + double saved_bias = filter.clock_bias_m; + assert(gps_pps_time_filter_update( + &filter, 25000000u, COUNTER_HZ, 5000.0, true, 10.0, 1.0, 1.0 / 32.0, 1u, + &filtered, &innovation) == GPS_PPS_TIME_FILTER_REJECTED); + assert(filter.epoch_counter == saved_epoch); + assert(filter.clock_bias_m == saved_bias); + assert(gps_pps_time_filter_update( + &filter, 0u, COUNTER_HZ, 100.0, true, 10.0, 1.0, + 1.0 / 32.0, 1u, &filtered, &innovation) == + GPS_PPS_TIME_FILTER_REJECTED); + assert(filter.epoch_counter == saved_epoch); + assert(filter.clock_bias_m == saved_bias); + + assert(gps_pps_time_filter_update( + &filter, 375000000u, COUNTER_HZ, 130.0, true, 10.0, 1.0, 1.0 / 32.0, 1u, + &filtered, &innovation) == GPS_PPS_TIME_FILTER_TRACKING); + assert(fabs(filtered - 130.0) < 1.0e-12); + + /* A gap beyond the bounded Doppler-prediction horizon must still reset + * the filter and require a safe coarse alignment. */ + assert(gps_pps_time_filter_update( + &filter, 4250000000ULL, COUNTER_HZ, 440.0, true, 10.0, + 1.0, 1.0 / 32.0, 1u, &filtered, &innovation) == + GPS_PPS_TIME_FILTER_COARSE); + assert(fabs(filtered - 440.0) < 1.0e-12); +} + +static void test_bias_ramp_without_doppler_uses_bounded_fallback(void) +{ + static const double noise_m[] = { + 80.0, -110.0, 45.0, -25.0, 125.0, -70.0, 35.0, -95.0, + 60.0, -40.0, 105.0, -85.0, 20.0, -15.0, 75.0, -55.0 + }; + gps_pps_time_filter_t filter; + gps_pps_time_filter_init(&filter); + + double squared_error_sum = 0.0; + unsigned int tracked = 0u; + for(unsigned int update = 0u; update < 500u; update++) + { + double time_sec = 0.2 * (double)update; + double true_bias_m = 1200.0 + 475.0 * time_sec; + double measured_bias_m = + true_bias_m + + noise_m[update % (sizeof(noise_m) / sizeof(noise_m[0]))]; + double filtered_bias_m = 0.0; + double innovation_m = 0.0; + gps_pps_time_filter_result_t result = gps_pps_time_filter_update( + &filter, + (uint64_t)llround(time_sec * COUNTER_HZ), + COUNTER_HZ, + measured_bias_m, + false, + 0.0, + 5.0, + 1.0 / 32.0, + 16u, + &filtered_bias_m, + &innovation_m); + assert(result != GPS_PPS_TIME_FILTER_REJECTED); + if((result == GPS_PPS_TIME_FILTER_TRACKING) && (update >= 200u)) + { + double error_m = filtered_bias_m - true_bias_m; + squared_error_sum += error_m * error_m; + tracked++; + } + } + assert(tracked > 250u); + assert(filter.drift_initialized); + assert(fabs(filter.clock_drift_mps) < 10000.0); + assert(sqrt(squared_error_sum / (double)tracked) < 45.0); +} + +static void test_reference_geometry_defines_bias_drift(void) +{ + const double receiver_position_m[3] = {0.0, 0.0, 0.0}; + const double receiver_velocity_mps[3] = {10.0, -2.0, 1.0}; + const double satellite_position_m[3] = {20200000.0, 0.0, 0.0}; + const double satellite_velocity_mps[3] = {140.0, 30.0, -10.0}; + double bias_drift_mps = 0.0; + + /* LOS is +X, so WLS datum drift is -(140 - 10) = -130 m/s. */ + assert(gps_pps_reference_bias_drift( + receiver_position_m, + receiver_velocity_mps, + satellite_position_m, + satellite_velocity_mps, + 0.0, + &bias_drift_mps)); + assert(fabs(bias_drift_mps + 130.0) < 1.0e-12); + + /* Broadcast satellite-clock drift is part of the position observation. */ + assert(gps_pps_reference_bias_drift( + receiver_position_m, + receiver_velocity_mps, + satellite_position_m, + satellite_velocity_mps, + 1.0e-9, + &bias_drift_mps)); + assert(fabs( + bias_drift_mps - + (-130.0 + 299792458.0 * 1.0e-9)) < 1.0e-12); + + double invalid_satellite_position[3] = {0.0, 0.0, 0.0}; + assert(!gps_pps_reference_bias_drift( + receiver_position_m, + receiver_velocity_mps, + invalid_satellite_position, + satellite_velocity_mps, + 0.0, + &bias_drift_mps)); +} + +static void test_reference_selection_is_stable(void) +{ + const unsigned int ids[] = {3u, 8u, 17u}; + const double captures[] = {102.0, 100.0, 104.0}; + unsigned int index = 99u; + + assert(gps_pps_select_reference( + ids, captures, 3u, false, 0u, &index)); + assert(index == 1u); + + /* A newly earlier satellite must not move an existing WLS datum. */ + assert(gps_pps_select_reference( + ids, captures, 3u, true, 3u, &index)); + assert(index == 0u); + + /* Loss of the preferred satellite selects the earliest remaining one. */ + assert(gps_pps_select_reference( + ids, captures, 3u, true, 25u, &index)); + assert(index == 1u); + + double invalid_captures[] = {102.0, NAN, 104.0}; + assert(!gps_pps_select_reference( + ids, invalid_captures, 3u, false, 0u, &index)); +} + +int main(void) +{ + test_drift_prediction_rejects_bias_noise(); + test_quality_gap_and_outlier_handling(); + test_bias_ramp_without_doppler_uses_bounded_fallback(); + test_reference_geometry_defines_bias_drift(); + test_reference_selection_is_stable(); + puts("gps_pps_time_filter tests passed"); + return 0; +} diff --git a/soc/software/gps_system/tests/test_gps_time_anchor.c b/soc/software/gps_system/tests/test_gps_time_anchor.c index 5f8b0d3..826d045 100644 --- a/soc/software/gps_system/tests/test_gps_time_anchor.c +++ b/soc/software/gps_system/tests/test_gps_time_anchor.c @@ -1,10 +1,11 @@ #include "gps/gps_time_anchor.h" +#include "gps/Navigation.h" #include #include #include -#define SAMPLE_RATE_HZ 16368000.0 +#define SAMPLE_RATE_HZ GPS_RF_SAMPLE_RATE_HZ static int failures; @@ -99,6 +100,89 @@ static void invalid_inputs_are_rejected(void) "out-of-range HOW was accepted"); } +static void physical_rf_rate_avoids_six_second_sawtooth(void) +{ + const double elapsed_sec = 6.0; + const double samples = GPS_RF_SAMPLE_RATE_HZ * elapsed_sec; + const double legacy_elapsed_sec = samples / 16368000.0; + const double corrected_elapsed_sec = samples / GPS_RF_SAMPLE_RATE_HZ; + + CHECK(fabs((legacy_elapsed_sec - elapsed_sec) * 1.0e9) > 6900.0, + "legacy 16.368 MHz conversion did not expose HOW sawtooth"); + CHECK(fabs((corrected_elapsed_sec - elapsed_sec) * 1.0e9) < 0.001, + "physical RF rate left a six-second timing ramp"); +} + +static void periodic_event_time_removes_receive_range_rate(void) +{ + const uint32_t anchor = 700000000u; + const double physical_elapsed_sec = 5.8 + 8.0e-6; + const uint32_t event = + anchor + (uint32_t)llround(physical_elapsed_sec * SAMPLE_RATE_HZ); + int week = 0; + double tow_sec = 0.0; + double elapsed_sec = 0.0; + + CHECK(gps_time_from_periodic_sample_anchor( + 380, 469362.0, anchor, (double)anchor, + event, (double)event, SAMPLE_RATE_HZ, 120.0, + 0.2, 0.005, &week, &tow_sec, &elapsed_sec), + "periodic NAV event with range-rate delay was rejected"); + CHECK(week == 380, "periodic NAV event changed GPS week"); + CHECK(fabs(tow_sec - 469367.8) < 1.0e-9, + "periodic NAV event retained receive-time range-rate error"); + CHECK(fabs(elapsed_sec - 5.8) < 1.0e-12, + "periodic NAV event did not report snapped elapsed time"); + + const uint32_t off_grid_event = + anchor + (uint32_t)llround(5.82 * SAMPLE_RATE_HZ); + CHECK(!gps_time_from_periodic_sample_anchor( + 380, 469362.0, anchor, (double)anchor, + off_grid_event, (double)off_grid_event, + SAMPLE_RATE_HZ, 120.0, 0.2, 0.005, + &week, &tow_sec, &elapsed_sec), + "off-grid NAV event was accepted"); +} + +static void fractional_code_phase_corrects_capture_counter(void) +{ + const uint64_t capture_counter = 1000000u; + uint64_t corrected_counter = 0u; + double correction_sec = 0.0; + + CHECK(gps_time_correct_capture_counter( + capture_counter, 0.75, SAMPLE_RATE_HZ, 125000000.0, + &corrected_counter, &correction_sec), + "valid fractional code phase did not correct PPS capture"); + /* 0.75 RF sample is 45.82 ns, rounded onto the 8 ns PPS counter grid. */ + CHECK(corrected_counter == capture_counter - 6u, + "fractional code phase used the wrong capture-counter correction"); + CHECK(fabs(correction_sec - 48.0e-9) < 1.0e-15, + "capture correction did not report the applied integer tick"); + + CHECK(gps_time_correct_capture_counter( + capture_counter, 0.0, SAMPLE_RATE_HZ, 125000000.0, + &corrected_counter, &correction_sec) && + (corrected_counter == capture_counter), + "zero code residue changed the capture counter"); + CHECK(!gps_time_correct_capture_counter( + capture_counter, -0.1, SAMPLE_RATE_HZ, 125000000.0, + &corrected_counter, NULL), + "negative fractional sample delay was accepted"); + CHECK(!gps_time_correct_capture_counter( + capture_counter, 1.1, SAMPLE_RATE_HZ, 125000000.0, + &corrected_counter, NULL), + "capture correction larger than one RF sample was accepted"); + CHECK(!gps_time_correct_capture_counter( + 2u, 1.0, SAMPLE_RATE_HZ, 125000000.0, + &corrected_counter, NULL), + "capture-counter underflow was accepted"); + CHECK(!gps_time_correct_capture_counter( + UINT64_MAX, 1.0, 1.0, 1.0e30, + &corrected_counter, NULL), + "capture correction beyond llround range was accepted"); +} + int main(void) { normal_and_skipped_subframes_are_consistent(); @@ -106,6 +190,9 @@ int main(void) temporary_week_can_upgrade_on_first_subframe_one(); captured_future_jump_is_rejected_and_recovers(); invalid_inputs_are_rejected(); + physical_rf_rate_avoids_six_second_sawtooth(); + periodic_event_time_removes_receive_range_rate(); + fractional_code_phase_corrects_capture_counter(); if(failures == 0) { puts("gps_time_anchor tests passed"); diff --git a/soc/software/pps_python/README.md b/soc/software/pps_python/README.md index ce6d610..b65115f 100644 --- a/soc/software/pps_python/README.md +++ b/soc/software/pps_python/README.md @@ -7,7 +7,7 @@ event-driven Python modelidir. Bu bir genel amaçlı GNSS simülatörü değildir. Özellikle aşağıdaki proje davranışlarını taklit eder: -- RF `ABS_SAMPLE` sayacı: `16.368 MHz`, 32 bit ve wrap eden sayaç. +- RF `ABS_SAMPLE` sayacı: `16.367981 MHz`, 32 bit ve wrap eden sayaç. - Tracking/PPS sayacı: `125 MHz` free-running sayaç. - Navigation solve periyodu: `NAV_SOLVE_TIMER=10`, yani `200 ms`. - Ortak navigation bit transmit epoch'u ve uyduların farklı propagation @@ -18,11 +18,21 @@ davranışlarını taklit eder: corrected_abs = ABS_SAMPLE - REM_CODE_PHASE / 1.023e6 ``` +- Integer RF sample kenarinda yakalanan 125 MHz counter da ayni code + boundary'ye geri cekilir: + + ```text + capture_correction_ticks = + round((REM_CODE_PHASE / 1.023e6) * (125e6 / 16.367981e6)) + corrected_capture = raw_capture - capture_correction_ticks + ``` + + Model ve C ayni 8 ns integer mailbox yuvarlamasini kullanir. - `gps_unwrap_corrected_abs_sample()` ile aynı signed modulo-32 unwrap. - `gps_ip.c` ile aynı pseudorange datum'u: ```text - precise_ms_i = abs_i / 16368 - minimum + 68.802 + precise_ms_i = abs_i / 16367.981 - reference + 68.802 P_i = precise_ms_i * c / 1000 + satellite_clock_i * c ``` @@ -45,6 +55,8 @@ Software dizininden: ```bash python3 pps_python/simulate_pps.py python3 -m unittest pps_python/test_pps_model.py +python3 pps_python/simulate_adaptive_pps.py +python3 -m unittest pps_python/test_pps_adaptive.py ``` Kısa veya farklı PI koşusu: @@ -92,11 +104,12 @@ olarak yazılır; simülasyon sonuç sayfaları `output/` altındaki PNG'leri ku ## Modeldeki doğru zaman bağı -Mevcut observation denkleminde WLS bias doğrudan PPS fazı değildir. Minimum -kanal için yaklaşık ilişki: +Mevcut observation denkleminde WLS bias doğrudan PPS fazı değildir. İlk +çözümde seçilen ve mevcut kaldığı sürece korunan referans kanal için yaklaşık +ilişki: ```text -b/c = STARTOFFSET - signal_flight_time_min +b/c = STARTOFFSET - signal_flight_time_ref ``` Bu nedenle doğru GPS referansı: @@ -105,7 +118,7 @@ Bu nedenle doğru GPS referansı: GPS_TIME_AT_CAPTURE = transmit_epoch_TOW + STARTOFFSET - b/c ``` -ile kurulur ve minimum kanala ait `tracking_tick_125` ile eşlenir. Referans +ile kurulur ve referans kanala ait `tracking_tick_125` ile eşlenir. Referans uydu değiştiğinde raw `b/c` milisaniyeler seviyesinde basamak yapabilir; TOW ve eşlenmiş timestamp ile oluşturulan GPS zamanı ise sürekli kalır. Üretilen ilk grafik bu farkı açıkça gösterir. @@ -148,14 +161,16 @@ bu üç etkiyi ayrı gösterir. 1. Raw 125 MHz counter resetten itibaren free-running çalışır. 2. İlk geçerli HOW week/6-second mutlak zaman etiketini sağlar; ilk hardware delivery epoch'u için 160 ms sabit readout düzeltmesi ayrıca eklenir. -3. İlk coherent WLS çözümünde minimum corrected-ABS kanalının uçuş süresi +3. İlk coherent WLS çözümünde minimum corrected-ABS kanalı datum olarak + seçilir; aynı SVID mevcut kaldığı sürece onun uçuş süresi `STARTOFFSET - b/c` ile elde edilir. 4. Bu receive GPS zamanı aynı kanalın yakalanmış 125 MHz timestamp'ıyla eşlenerek PPS accumulator coarse-align edilir. 5. Sonraki çözümler aynı eşleşmeden residual phase error üretip PI'yi besler. -Minimum kanal yalnız zaman datum'unu seçer; `b` bütün WLS uydularıyla beraber -çözülür. İstenirse çözüm sonrası her uydu için +Referans kanal yalnız zaman datum'unu seçer; `b` bütün WLS uydularıyla beraber +çözülür. Referans kaybolursa yeni en erken kanal seçilir ve filtre bilinçli +coarse re-anchor edilir. İstenirse çözüm sonrası her uydu için `(rho + iono + tropo) / c - satellite_clock` hesaplanıp kendi timestamp'ına bağlanabilir. Doğru hizalamada bunların hepsi aynı receiver time mapping'ini vermelidir; bu ayrıca güçlü bir coherency testi olur. @@ -198,6 +213,56 @@ Bunlar Q2.46 donanım katsayılarının ilk çalışma değerleridir. Karttan cl serisi ve oscillator drift ölçüldükten sonra Allan deviation/phase-noise davranışına göre yeniden ayarlanmalıdır. +Donanım loop filter'ı bu PI yapısının kendisidir. Ayrı bir phase-IIR veya +derivative/D kolu yoktur. Küçük yazılım filtresi yalnız WLS clock-bias +ölçümünün residual gürültüsünü Doppler clock-drift prediction sonrasında +sınırlar; PI dinamiğinin yerine geçmez. Acquisition sırasında `0.12 Hz`, +precision modunda `0.035 Hz`, zayıf geometride robust `0.012 Hz` doğal frekans +kullanılır. `Kp`, `Ki`, `20 ppm` slew limiti ve lock politikası çalışma anında +profil ile değişir; integrator normal profil geçişlerinde korunur. Valid lock +için beş ardışık residual'ın 125 MHz saatinde sekiz tick (`64 ns`) içinde +olması gerekir. Üretim profilleri tek seyrek WLS sıçramasında fiziksel pulse +kaçırmamak için lock'u ancak `128 tick = 1.024 us` üstünde bırakır; watchdog, +quality holdover ve slew clamp daha büyük arızaları ayrıca sınırlar. + +## Adaptif PPS senaryo matrisi + +Tam fixed-point/holdover matrisi: + +```bash +python3 -m pps_python.analyze_pps_scenarios +``` + +Model, 125 MHz çıkış grid'ini de PPS kenarına quantize eder. 2026-07-25 +regresyonunda ayrı donanım IIR olmadan: + +| Senaryo | Lock | İlk lock p95 | Edge jitter p95 | +|---|---:|---:|---:| +| temiz precision | 30/30 | 10.80 s | 5.50 ns | +| 320 ns kart noise + outlier | 50/50 | 12.31 s | 13.62 ns | +| 650 ns zayıf geometri | 30/30 | 13.77 s | 12.31 ns | +| başlangıç 1.1 us noise, sonra temiz | 30/30 | 16.93 s | 8.45 ns | +| aralıklı noise burst, sonra recovery | 30/30 | 12.00 s | 10.82 ns | +| iki uydu-kaybi aralığı ve recovery | 30/30 | 11.60 s | 12.57 ns | +| Doppler kaybı ve recovery | 30/30 | 11.60 s | 12.41 ns | +| +18 ppm, slew sınırına yakın | 30/30 | 18.20 s | 12.01 ns | +| +25 ppm, 20 ppm sınırının dışında | 0/10 | lock yok | geçerli edge yok | + +Ek olarak kısa/uzun WLS kesintisi, quality profile geçişi, multipath basamağı +ve hiç Doppler olmayan fallback yolu test edilir. Linux +`test_gps_pps_adaptive_loop.c` aynı geçişli ortamları C implementasyonu ve +Q32/Q46 aritmetiğiyle çoklu seed altında tekrarlar. `test_gps_pps_quality.c` +NaN, dört uydunun altına düşme, eşik sınırları ve histerezisi; +`test_gps_pps_time_filter.c` drift prediction, gap/re-anchor ve hard-outlier +reddini sınar. + +İki zaman eşiği bilinçli olarak ayrıdır: iki saniyelik watchdog gözlem yokken +PPS validliğini holdover'a indirir; aynı referans SVID için Doppler-predicted +clock-bias ve PI state'i 30 saniyeye kadar korunur. Modelde 15 saniyelik WLS +ve uydu kaybı boşlukları ek coarse commit/unlock üretmeden geri döner. +30 saniyeyi aşan boşluk veya referans SVID değişimi güvenli coarse re-anchor +yapar. + ## C kodu analizi sırasında görülen önemli noktalar 1. `leastSquaresSTD()` artık aynı başarılı çözümün `x/y/z/b` vektörünü çağırana @@ -210,9 +275,10 @@ davranışına göre yeniden ayarlanmalıdır. 3. Pseudorange her epoch'ta `minimum` ile yeniden normalize edildiği için RF counter'ın mutlak fazı WLS observation'da iptal olur. RF frequency error ise uyduların arrival-time farklarını ölçeklediği için tamamen iptal olmaz. -4. PPS tarafı array'in ilk elemanını referans kabul etmiyor; minimum corrected - ABS kanalının TOW'u ve aynı nav-solve olayındaki C125 capture'ı birlikte - taşınıyor. +4. PPS tarafı array'in ilk elemanını referans kabul etmiyor; ilk geçerli + çözümde minimum corrected ABS kanalı seçiliyor ve aynı SVID coherent + snapshot'ta kaldığı sürece datum sabit tutuluyor. O kanalın TOW'u ve aynı + nav-solve olayındaki C125 capture'ı birlikte taşınıyor. 5. `POSTPONE_TOW=0.160`, hardware'ın mevcut subframe'i doğrulamak için takip eden subframe'den 8 bit daha beklemesinden oluşan sabit readout gecikmesidir. Uydu konum/saat epoch'u buna göre 160 ms ileri alınır. Bu değer signal diff --git a/soc/software/pps_python/adaptive_pps.py b/soc/software/pps_python/adaptive_pps.py new file mode 100644 index 0000000..477b4ea --- /dev/null +++ b/soc/software/pps_python/adaptive_pps.py @@ -0,0 +1,718 @@ +"""Bit-accurate event model for the adaptive PPS control path. + +The WLS/navigation model in :mod:`pps_model` validates timestamp ownership. +This module focuses on the control corner cases seen on the board: noisy or +missing WLS time observations, quality-driven coefficient changes, fixed-point +rounding, lock acquisition/release and holdover/re-acquisition. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from enum import IntEnum +import math +from typing import Iterable + + +TRACKING_HZ = 125_000_000 +SPEED_OF_LIGHT_MPS = 299_792_458.0 +REFERENCE_WATCHDOG_SEC = 2.0 +MAX_PREDICTION_GAP_SEC = 30.0 +Q32 = 1 << 32 +Q46 = 1 << 46 +MAX_SIGNED_64 = (1 << 63) - 1 +MIN_SIGNED_64 = -(1 << 63) + + +class Profile(IntEnum): + HOLDOVER = 0 + ROBUST = 1 + PRECISION = 2 + + +@dataclass(frozen=True) +class QualityMetrics: + tdop: float + gdop: float + clock_bias_sigma_m: float + mean_cn0_dbhz: float + minimum_cn0_dbhz: float + chi_square_ratio: float + velocity_residual_rms_mps: float + used_satellites: int + solution_valid: bool = True + residual_test_valid: bool = True + velocity_valid: bool = True + fde_applied: bool = False + + +@dataclass(frozen=True) +class LoopProfile: + natural_frequency_hz: float + damping: float + max_slew_ppm: float + lock_acquire_ticks: int + lock_release_ticks: int + lock_acquire_updates: int + software_bias_gain: float + software_settle_updates: int + + def fixed_coefficients(self, update_period_s: float) -> tuple[int, int, int]: + omega = 2.0 * math.pi * self.natural_frequency_hz + kp_q46 = round((2.0 * self.damping * omega / TRACKING_HZ) * Q46) + ki_q46 = round( + (omega * omega * update_period_s / TRACKING_HZ) * Q46 + ) + max_slew_q32 = round(self.max_slew_ppm * 1.0e-6 * Q32) + return kp_q46, ki_q46, max_slew_q32 + + +PROFILES = { + Profile.HOLDOVER: LoopProfile( + 0.0, 1.0, 20.0, 8, 128, 8, 1.0 / 32.0, 16 + ), + Profile.ROBUST: LoopProfile( + 0.012, 1.0, 20.0, 8, 128, 8, 1.0 / 32.0, 16 + ), + Profile.PRECISION: LoopProfile( + 0.035, 0.9, 20.0, 8, 128, 5, 1.0 / 32.0, 12 + ), +} + +ACQUIRE_PROFILE = LoopProfile( + 0.12, 0.9, 20.0, 8, 128, 5, 1.0 / 16.0, 12 +) + + +def _finite_metrics(metrics: QualityMetrics) -> bool: + return all( + math.isfinite(value) + for value in ( + metrics.tdop, + metrics.gdop, + metrics.clock_bias_sigma_m, + metrics.mean_cn0_dbhz, + metrics.minimum_cn0_dbhz, + metrics.velocity_residual_rms_mps, + ) + ) + + +def classify_quality(metrics: QualityMetrics) -> Profile: + invalid = ( + not _finite_metrics(metrics) + or not metrics.solution_valid + or metrics.used_satellites < 4 + or metrics.tdop < 0.0 + or metrics.gdop < 0.0 + or metrics.clock_bias_sigma_m < 0.0 + ) + if invalid: + return Profile.HOLDOVER + + residual_ok = ( + metrics.residual_test_valid + and math.isfinite(metrics.chi_square_ratio) + and metrics.chi_square_ratio <= 1.0 + ) or (not metrics.residual_test_valid and metrics.used_satellites == 4) + robust = ( + residual_ok + and metrics.tdop <= 8.0 + and metrics.gdop <= 12.0 + and metrics.clock_bias_sigma_m <= 100.0 + and metrics.mean_cn0_dbhz >= 22.0 + and metrics.minimum_cn0_dbhz >= 14.0 + and ( + not metrics.velocity_valid + or metrics.velocity_residual_rms_mps <= 3.0 + ) + ) + if not robust: + return Profile.HOLDOVER + + precision = ( + metrics.used_satellites >= 6 + and metrics.residual_test_valid + and not metrics.fde_applied + and metrics.tdop <= 3.0 + and metrics.gdop <= 6.0 + and metrics.clock_bias_sigma_m <= 35.0 + and metrics.mean_cn0_dbhz >= 30.0 + and metrics.minimum_cn0_dbhz >= 20.0 + and metrics.chi_square_ratio <= 0.75 + and metrics.velocity_valid + and metrics.velocity_residual_rms_mps <= 1.0 + ) + return Profile.PRECISION if precision else Profile.ROBUST + + +class QualitySelector: + """Three-profile selector with asymmetric transition hysteresis.""" + + def __init__(self) -> None: + self.profile = Profile.HOLDOVER + self.candidate = Profile.HOLDOVER + self.candidate_updates = 0 + + def update(self, metrics: QualityMetrics) -> tuple[Profile, bool]: + target = classify_quality(metrics) + if target == self.profile: + self.candidate = target + self.candidate_updates = 0 + return self.profile, False + if target != self.candidate: + self.candidate = target + self.candidate_updates = 1 + else: + self.candidate_updates += 1 + required = 8 if target > self.profile else 3 + if self.candidate_updates >= required: + self.profile = target + self.candidate_updates = 0 + return self.profile, True + return self.profile, False + + +@dataclass(frozen=True) +class Observation: + time_s: float + timing_error_s: float + clock_drift_error_mps: float + quality: QualityMetrics + clock_drift_valid: bool = True + reference_range_rate_mps: float = 0.0 + reference_id: int = 1 + reference_datum_offset_m: float = 0.0 + + +@dataclass +class AdaptiveTrace: + time_s: list[float] + phase_error_s: list[float] + raw_measurement_error_s: list[float] + software_filtered_error_s: list[float] + raw_loop_error_s: list[float] + control_loop_error_s: list[float] + frequency_command_ppm: list[float] + profile: list[Profile] + locked: list[bool] + coarse_commits: int + normal_commits: int + rejected_observations: int + lock_events: int + unlock_events: int + + def locked_fraction(self) -> float: + return sum(self.locked) / len(self.locked) if self.locked else 0.0 + + def first_lock_time(self) -> float | None: + for time_s, locked in zip(self.time_s, self.locked): + if locked: + return time_s + return None + + def one_second_jitter_rms(self, *, after_s: float = 0.0) -> float: + selected = [ + (time_s, phase_s) + for time_s, phase_s, locked in zip( + self.time_s, self.phase_error_s, self.locked + ) + if locked and time_s >= after_s + ] + if len(selected) < 2: + return math.nan + differences = [] + previous_index = 0 + for index, (time_s, phase_s) in enumerate(selected): + while ( + previous_index < index + and selected[previous_index][0] < time_s - 1.0 - 1.0e-9 + ): + previous_index += 1 + if ( + previous_index < index + and abs(selected[previous_index][0] - (time_s - 1.0)) < 1.0e-9 + ): + differences.append(phase_s - selected[previous_index][1]) + if not differences: + return math.nan + return math.sqrt( + sum(value * value for value in differences) / len(differences) + ) + + def one_second_edge_jitter_rms(self, *, after_s: float = 0.0) -> float: + """Return 1 s PPS-edge jitter including the 125 MHz output grid.""" + selected = { + round(time_s, 9): ( + round(-phase_s * TRACKING_HZ) / TRACKING_HZ + ) + for time_s, phase_s, locked in zip( + self.time_s, self.phase_error_s, self.locked + ) + if locked + and time_s >= after_s + and abs(time_s - round(time_s)) < 1.0e-9 + } + differences = [ + edge_error - selected[round(time_s - 1.0, 9)] + for time_s, edge_error in selected.items() + if round(time_s - 1.0, 9) in selected + ] + if not differences: + return math.nan + return math.sqrt( + sum(value * value for value in differences) / len(differences) + ) + + def phase_rms(self, *, after_s: float = 0.0) -> float: + values = [ + phase_s + for time_s, phase_s, locked in zip( + self.time_s, self.phase_error_s, self.locked + ) + if locked and time_s >= after_s + ] + if not values: + return math.nan + return math.sqrt(sum(value * value for value in values) / len(values)) + + def settling_time( + self, + *, + rms_limit_s: float, + window_s: float = 10.0, + ) -> float | None: + """First all-locked window whose phase RMS meets ``rms_limit_s``.""" + for end_index, end_time_s in enumerate(self.time_s): + start_time_s = end_time_s - window_s + if start_time_s < 0.0: + continue + window = [ + (phase_s, locked) + for time_s, phase_s, locked in zip( + self.time_s[: end_index + 1], + self.phase_error_s[: end_index + 1], + self.locked[: end_index + 1], + ) + if time_s >= start_time_s - 1.0e-9 + ] + if not window or not all(locked for _, locked in window): + continue + rms = math.sqrt( + sum(phase_s * phase_s for phase_s, _ in window) + / len(window) + ) + if rms <= rms_limit_s: + return end_time_s + return None + + +class FixedPointAdaptivePps: + """Event-driven model of the Q32/Q46 RTL plus the small clock-bias filter.""" + + def __init__( + self, + *, + update_period_s: float = 0.2, + oscillator_ppm: float = 1.65, + adaptive: bool = True, + software_filter: bool = True, + receiver_clock_bias_m: float = 1200.0, + receiver_clock_drift_mps: float = 475.0, + ) -> None: + self.update_period_s = update_period_s + self.oscillator_fraction = oscillator_ppm * 1.0e-6 + self.adaptive = adaptive + self.software_filter = software_filter + self.receiver_clock_bias_m = receiver_clock_bias_m + self.receiver_clock_drift_mps = receiver_clock_drift_mps + self.selector = QualitySelector() + self.profile = Profile.HOLDOVER if adaptive else Profile.PRECISION + self.parameters = PROFILES[self.profile] + self.kp_q46, self.ki_q46, self.max_slew_q32 = ( + self.parameters.fixed_coefficients(update_period_s) + ) + + self.current_time_s = 0.0 + self.raw_tick_remainder = 0.0 + self.phase_q32 = 0 + self.rate_word_q32 = 0 + self.integrator_q32 = 0 + self.aligned = False + self.locked = False + self.lock_good_updates = 0 + self.last_observation_time_s: float | None = None + self.software_error_s = 0.0 + self.software_bias_m = 0.0 + self.software_clock_drift_mps = 0.0 + self.software_previous_measurement_m = 0.0 + self.software_drift_anchor_time_s = 0.0 + self.software_drift_anchor_measurement_m = 0.0 + self.software_drift_initialized = False + self.software_updates = 0 + self.software_reference_id: int | None = None + self.reference_watchdog_holdover = True + + self.trace = AdaptiveTrace( + [], [], [], [], [], [], [], [], [], 0, 0, 0, 0, 0 + ) + + @staticmethod + def _clamp(value: int, limit: int) -> int: + return max(-limit, min(limit, value)) + + @staticmethod + def _seconds_to_phase_q32(seconds: float) -> int: + value = round(seconds * TRACKING_HZ * Q32) + return max(MIN_SIGNED_64, min(MAX_SIGNED_64, value)) + + @staticmethod + def _phase_q32_to_seconds(value: int) -> float: + return value / (TRACKING_HZ * Q32) + + def _apply_profile(self, profile: Profile) -> None: + self.profile = profile + self._select_loop_parameters() + + def _select_loop_parameters(self) -> None: + parameters = ( + ACQUIRE_PROFILE + if self.adaptive and + self.profile != Profile.HOLDOVER and not self.locked + else PROFILES[self.profile] + ) + if parameters == self.parameters: + return + self.parameters = parameters + self.kp_q46, self.ki_q46, self.max_slew_q32 = ( + self.parameters.fixed_coefficients(self.update_period_s) + ) + + def _advance(self, time_s: float) -> None: + elapsed_s = time_s - self.current_time_s + if elapsed_s < -1.0e-15: + raise ValueError("observations are not time ordered") + if elapsed_s <= 0.0: + return + true_ticks = elapsed_s * TRACKING_HZ + raw_ticks_float = ( + true_ticks * (1.0 + self.oscillator_fraction) + + self.raw_tick_remainder + ) + raw_ticks = int(math.floor(raw_ticks_float)) + self.raw_tick_remainder = raw_ticks_float - raw_ticks + self.phase_q32 += ( + raw_ticks * (Q32 + self.rate_word_q32) + - round(true_ticks * Q32) + ) + self.phase_q32 = max( + MIN_SIGNED_64, min(MAX_SIGNED_64, self.phase_q32) + ) + self.current_time_s = time_s + + def _software_filter_update(self, observation: Observation) -> tuple[bool, bool]: + if ( + not math.isfinite(observation.timing_error_s) + or ( + observation.clock_drift_valid + and not math.isfinite(observation.clock_drift_error_mps) + ) + ): + self.trace.rejected_observations += 1 + return False, False + + if self.software_reference_id != observation.reference_id: + # WLS bias has a reference-dependent datum. A satellite change is + # a deliberate coarse re-anchor, never an outlier to interpolate. + self.last_observation_time_s = None + self.software_updates = 0 + self.software_drift_initialized = False + self.software_reference_id = observation.reference_id + + true_bias_m = ( + self.receiver_clock_bias_m + + observation.reference_datum_offset_m + - observation.reference_range_rate_mps * observation.time_s + ) + measured_bias_m = ( + true_bias_m - observation.timing_error_s * SPEED_OF_LIGHT_MPS + ) + measured_drift_mps = ( + -observation.reference_range_rate_mps + + observation.clock_drift_error_mps + ) + if self.last_observation_time_s is None: + self.software_bias_m = measured_bias_m + self.software_clock_drift_mps = ( + measured_drift_mps + if observation.clock_drift_valid + else 0.0 + ) + self.software_previous_measurement_m = measured_bias_m + self.software_drift_anchor_time_s = observation.time_s + self.software_drift_anchor_measurement_m = measured_bias_m + self.software_drift_initialized = observation.clock_drift_valid + self.software_error_s = observation.timing_error_s + self.software_updates = 1 + self.last_observation_time_s = observation.time_s + return True, True + interval_s = observation.time_s - self.last_observation_time_s + if interval_s < 0.04: + self.trace.rejected_observations += 1 + return False, False + # The two-second reference watchdog controls PPS validity, not filter + # state lifetime. Keeping the Doppler-predicted bias and the hardware + # PI state across a bounded WLS scheduling/signal gap avoids a coarse + # phase step when the same datum satellite returns. A reference + # change is handled above and a genuinely long gap still re-anchors. + if interval_s > MAX_PREDICTION_GAP_SEC: + self.software_bias_m = measured_bias_m + self.software_clock_drift_mps = ( + measured_drift_mps + if observation.clock_drift_valid + else 0.0 + ) + self.software_previous_measurement_m = measured_bias_m + self.software_drift_anchor_time_s = observation.time_s + self.software_drift_anchor_measurement_m = measured_bias_m + self.software_drift_initialized = observation.clock_drift_valid + self.software_error_s = observation.timing_error_s + self.software_updates = 1 + self.last_observation_time_s = observation.time_s + return True, True + + if not self.software_filter: + self.software_bias_m = measured_bias_m + self.software_clock_drift_mps = ( + measured_drift_mps + if observation.clock_drift_valid + else self.software_clock_drift_mps + ) + self.software_previous_measurement_m = measured_bias_m + self.software_drift_initialized |= observation.clock_drift_valid + self.software_error_s = observation.timing_error_s + self.software_updates += 1 + self.last_observation_time_s = observation.time_s + return True, False + + measurement_delta_m = ( + measured_bias_m - self.software_previous_measurement_m + ) + if ( + not observation.clock_drift_valid + and abs(measurement_delta_m) + > 10_000.0 * interval_s + 1500.0 + ): + self.trace.rejected_observations += 1 + return False, False + + anchor_interval_s = ( + observation.time_s - self.software_drift_anchor_time_s + ) + fallback_drift_mps = max( + -10_000.0, + min( + 10_000.0, + ( + measured_bias_m + - self.software_drift_anchor_measurement_m + ) + / anchor_interval_s + if anchor_interval_s > 0.0 + else measurement_delta_m / interval_s, + ), + ) + if not self.software_drift_initialized: + self.software_clock_drift_mps = fallback_drift_mps + self.software_drift_initialized = True + + predicted_bias_m = ( + self.software_bias_m + + self.software_clock_drift_mps * interval_s + ) + innovation_m = measured_bias_m - predicted_bias_m + if abs(innovation_m) > 1500.0: + self.trace.rejected_observations += 1 + return False, False + + innovation_m = max(-300.0, min(300.0, innovation_m)) + self.software_bias_m = ( + predicted_bias_m + + self.parameters.software_bias_gain * innovation_m + ) + if observation.clock_drift_valid: + self.software_clock_drift_mps += 0.25 * ( + measured_drift_mps - self.software_clock_drift_mps + ) + else: + if self.software_updates < self.parameters.software_settle_updates: + self.software_clock_drift_mps = fallback_drift_mps + else: + self.software_clock_drift_mps += (1.0 / 16.0) * ( + fallback_drift_mps - self.software_clock_drift_mps + ) + self.software_previous_measurement_m = measured_bias_m + self.software_error_s = -( + self.software_bias_m - true_bias_m + ) / SPEED_OF_LIGHT_MPS + self.software_updates += 1 + self.last_observation_time_s = observation.time_s + coarse = ( + self.software_filter + and self.software_updates < self.parameters.software_settle_updates + ) + return True, coarse + + def _normal_commit(self, measurement_error_q32: int) -> tuple[int, int]: + raw_error_q32 = measurement_error_q32 - self.phase_q32 + raw_error_q32 = max( + MIN_SIGNED_64, min(MAX_SIGNED_64, raw_error_q32) + ) + + proportional = (raw_error_q32 * self.kp_q46) >> 46 + integrator_candidate = ( + self.integrator_q32 + + ((raw_error_q32 * self.ki_q46) >> 46) + ) + self.integrator_q32 = self._clamp( + integrator_candidate, self.max_slew_q32 + ) + self.rate_word_q32 = self._clamp( + self.integrator_q32 + proportional, self.max_slew_q32 + ) + + acquire = self.parameters.lock_acquire_ticks * Q32 + release = self.parameters.lock_release_ticks * Q32 + was_locked = self.locked + if abs(raw_error_q32) > release: + self.locked = False + self.lock_good_updates = 0 + elif not self.locked: + if abs(raw_error_q32) <= acquire: + self.lock_good_updates += 1 + if ( + self.lock_good_updates + >= self.parameters.lock_acquire_updates + ): + self.locked = True + self.lock_good_updates = 0 + else: + self.lock_good_updates = 0 + if self.locked and not was_locked: + self.trace.lock_events += 1 + elif was_locked and not self.locked: + self.trace.unlock_events += 1 + return raw_error_q32, raw_error_q32 + + def update(self, observation: Observation) -> None: + self._advance(observation.time_s) + if self.adaptive and observation.quality.solution_valid: + selected, changed = self.selector.update(observation.quality) + if changed or selected != self.profile: + self._apply_profile(selected) + if not observation.quality.solution_valid: + if ( + self.last_observation_time_s is None + or observation.time_s - self.last_observation_time_s + > REFERENCE_WATCHDOG_SEC + ): + self.reference_watchdog_holdover = True + self._record(observation, 0, 0) + return + if self.profile == Profile.HOLDOVER: + self.reference_watchdog_holdover = True + self._record(observation, 0, 0) + return + self._select_loop_parameters() + + accepted, coarse = self._software_filter_update(observation) + if not accepted: + if ( + self.last_observation_time_s is None + or observation.time_s - self.last_observation_time_s + > REFERENCE_WATCHDOG_SEC + ): + self.reference_watchdog_holdover = True + self._record(observation, 0, 0) + return + measurement_error_q32 = self._seconds_to_phase_q32( + self.software_error_s + ) + if coarse or not self.aligned: + was_locked = self.locked + self.phase_q32 = measurement_error_q32 + self.rate_word_q32 = 0 + self.integrator_q32 = 0 + self.aligned = True + self.locked = False + self.lock_good_updates = 0 + self.trace.coarse_commits += 1 + if was_locked: + self.trace.unlock_events += 1 + raw_error_q32 = 0 + control_error_q32 = 0 + self.reference_watchdog_holdover = True + else: + raw_error_q32, control_error_q32 = self._normal_commit( + measurement_error_q32 + ) + self.trace.normal_commits += 1 + self.reference_watchdog_holdover = False + self._record(observation, raw_error_q32, control_error_q32) + + def _record( + self, + observation: Observation, + raw_error_q32: int, + control_error_q32: int, + ) -> None: + self.trace.time_s.append(observation.time_s) + self.trace.phase_error_s.append( + self._phase_q32_to_seconds(self.phase_q32) + ) + self.trace.raw_measurement_error_s.append(observation.timing_error_s) + self.trace.software_filtered_error_s.append(self.software_error_s) + self.trace.raw_loop_error_s.append( + self._phase_q32_to_seconds(raw_error_q32) + ) + self.trace.control_loop_error_s.append( + self._phase_q32_to_seconds(control_error_q32) + ) + self.trace.frequency_command_ppm.append( + self.rate_word_q32 / Q32 * 1.0e6 + ) + self.trace.profile.append(self.profile) + # STATUS.LOCKED is retained internally during holdover, but external + # PPS validity is deliberately low. Scenario metrics use the valid + # lock contract rather than counting a held stale state as usable. + self.trace.locked.append( + self.locked + and self.profile != Profile.HOLDOVER + and not self.reference_watchdog_holdover + ) + + def finish(self, time_s: float) -> AdaptiveTrace: + self._advance(time_s) + return self.trace + + +def run_observations( + observations: Iterable[Observation], + *, + finish_time_s: float, + oscillator_ppm: float = 1.65, + adaptive: bool = True, + software_filter: bool = True, + receiver_clock_bias_m: float = 1200.0, + receiver_clock_drift_mps: float = 475.0, +) -> AdaptiveTrace: + model = FixedPointAdaptivePps( + oscillator_ppm=oscillator_ppm, + adaptive=adaptive, + software_filter=software_filter, + receiver_clock_bias_m=receiver_clock_bias_m, + receiver_clock_drift_mps=receiver_clock_drift_mps, + ) + for observation in observations: + model.update(observation) + return model.finish(finish_time_s) diff --git a/soc/software/pps_python/analyze_pps_scenarios.py b/soc/software/pps_python/analyze_pps_scenarios.py new file mode 100644 index 0000000..eeaac23 --- /dev/null +++ b/soc/software/pps_python/analyze_pps_scenarios.py @@ -0,0 +1,406 @@ +#!/usr/bin/env python3 +"""Embedded-oriented PPS closed-loop scenario and Monte-Carlo analysis. + +This is intentionally deterministic and dependency-free. It exercises the +same Q32/Q46 controller, quality hysteresis, Doppler-aided software pre-filter, +runtime profile changes, lock detector and 125 MHz edge quantization used by +the implementation plan. +""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +import math +import random +import statistics +from typing import Callable + +try: + from .adaptive_pps import ( + Observation, + Profile, + QualityMetrics, + run_observations, + ) +except ImportError: + from adaptive_pps import ( + Observation, + Profile, + QualityMetrics, + run_observations, + ) + + +@dataclass(frozen=True) +class Scenario: + name: str + duration_s: float + seeds: int + oscillator_ppm: float + noise_sigma: Callable[[float], float] + quality_profile: Callable[[float], Profile] + gaps: tuple[tuple[float, float], ...] = () + timing_step: Callable[[float], float] = lambda _time_s: 0.0 + outlier_period: int = 0 + outlier_size_s: float = 0.0 + drift_sigma_mps: float = 0.25 + drift_valid: bool = True + drift_valid_profile: Callable[[float], bool] | None = None + drop_period: int = 0 + invalid_intervals: tuple[tuple[float, float], ...] = () + reference_range_rate: Callable[[float], float] = lambda _time_s: 0.0 + reference_id: Callable[[float], int] = lambda _time_s: 1 + reference_datum_offset: Callable[[float], float] = ( + lambda _time_s: 0.0 + ) + + +@dataclass(frozen=True) +class ScenarioResult: + name: str + lock_success: int + runs: int + first_lock_p95_s: float + settle_p95_s: float + phase_rms_median_ns: float + edge_jitter_median_ns: float + edge_jitter_p95_ns: float + unlock_total: int + coarse_total: int + rejected_total: int + + +def quality(profile: Profile) -> QualityMetrics: + if profile == Profile.PRECISION: + return QualityMetrics( + 1.7, 3.2, 18.0, 38.0, 27.0, 0.35, 0.25, 7 + ) + if profile == Profile.ROBUST: + return QualityMetrics( + 5.0, 8.0, 65.0, 27.0, 18.0, 0.85, 1.8, 5 + ) + return QualityMetrics( + 12.0, 18.0, 180.0, 18.0, 10.0, 2.0, 4.0, 4 + ) + + +def _percentile(values: list[float], percentile: float) -> float: + if not values: + return math.nan + ordered = sorted(values) + position = (len(ordered) - 1) * percentile + lower = math.floor(position) + upper = math.ceil(position) + if lower == upper: + return ordered[lower] + fraction = position - lower + return ordered[lower] * (1.0 - fraction) + ordered[upper] * fraction + + +def observations(scenario: Scenario, seed: int) -> list[Observation]: + rng = random.Random(seed) + result: list[Observation] = [] + updates = round(scenario.duration_s / 0.2) + for update in range(updates): + time_s = update * 0.2 + if any(start <= time_s < stop for start, stop in scenario.gaps): + continue + if scenario.drop_period and update % scenario.drop_period == 0: + continue + timing_error_s = ( + rng.gauss(0.0, scenario.noise_sigma(time_s)) + + scenario.timing_step(time_s) + ) + if scenario.outlier_period and update % scenario.outlier_period == 0: + timing_error_s += rng.choice((-1.0, 1.0)) * scenario.outlier_size_s + quality_metrics = quality(scenario.quality_profile(time_s)) + solution_valid = not any( + start <= time_s < stop + for start, stop in scenario.invalid_intervals + ) + if not solution_valid: + quality_metrics = replace( + quality_metrics, + solution_valid=False, + used_satellites=2, + velocity_valid=False, + ) + drift_valid = ( + scenario.drift_valid_profile(time_s) + if scenario.drift_valid_profile is not None + else scenario.drift_valid + ) + if not drift_valid: + quality_metrics = replace( + quality_metrics, + velocity_valid=False, + velocity_residual_rms_mps=0.0, + ) + result.append( + Observation( + time_s=time_s, + timing_error_s=timing_error_s, + clock_drift_error_mps=rng.gauss( + 0.0, scenario.drift_sigma_mps + ), + quality=quality_metrics, + clock_drift_valid=drift_valid, + reference_range_rate_mps=scenario.reference_range_rate( + time_s + ), + reference_id=scenario.reference_id(time_s), + reference_datum_offset_m=scenario.reference_datum_offset( + time_s + ), + ) + ) + return result + + +def run_scenario(scenario: Scenario) -> ScenarioResult: + traces = [ + run_observations( + observations(scenario, seed), + finish_time_s=scenario.duration_s, + oscillator_ppm=scenario.oscillator_ppm, + ) + for seed in range(1, scenario.seeds + 1) + ] + first_locks = [ + value + for trace in traces + if (value := trace.first_lock_time()) is not None + ] + settle_times = [ + value + for trace in traces + if ( + value := trace.settling_time( + rms_limit_s=100.0e-9, window_s=10.0 + ) + ) + is not None + ] + analysis_start_s = max(30.0, scenario.duration_s * 0.70) + phase_rms = [ + value * 1.0e9 + for trace in traces + if math.isfinite( + value := trace.phase_rms(after_s=analysis_start_s) + ) + ] + edge_jitter = [ + value * 1.0e9 + for trace in traces + if math.isfinite( + value := trace.one_second_edge_jitter_rms( + after_s=analysis_start_s + ) + ) + ] + return ScenarioResult( + name=scenario.name, + lock_success=len(first_locks), + runs=len(traces), + first_lock_p95_s=_percentile(first_locks, 0.95), + settle_p95_s=_percentile(settle_times, 0.95), + phase_rms_median_ns=( + statistics.median(phase_rms) if phase_rms else math.nan + ), + edge_jitter_median_ns=( + statistics.median(edge_jitter) if edge_jitter else math.nan + ), + edge_jitter_p95_ns=_percentile(edge_jitter, 0.95), + unlock_total=sum(trace.unlock_events for trace in traces), + coarse_total=sum(trace.coarse_commits for trace in traces), + rejected_total=sum(trace.rejected_observations for trace in traces), + ) + + +def scenario_matrix() -> tuple[Scenario, ...]: + precision = lambda _time_s: Profile.PRECISION + robust = lambda _time_s: Profile.ROBUST + return ( + Scenario( + "clean_precision", 120.0, 30, 1.65, + lambda _time_s: 50.0e-9, precision, + ), + Scenario( + "board_noise_outliers", 180.0, 50, 1.65, + lambda _time_s: 320.0e-9, precision, + outlier_period=41, outlier_size_s=900.0e-9, + ), + Scenario( + "weak_geometry_robust", 240.0, 30, 1.65, + lambda _time_s: 650.0e-9, robust, + outlier_period=37, outlier_size_s=1.2e-6, + drift_sigma_mps=0.5, + ), + Scenario( + "short_dropouts", 180.0, 30, -3.0, + lambda _time_s: 320.0e-9, precision, + drop_period=7, + ), + Scenario( + "long_wls_gap", 180.0, 30, 1.65, + lambda _time_s: 320.0e-9, precision, + gaps=((60.0, 75.0),), + ), + Scenario( + "quality_transitions", 240.0, 30, 2.5, + lambda time_s: ( + 320.0e-9 + if time_s < 70.0 or time_s >= 150.0 + else 650.0e-9 + ), + lambda time_s: ( + Profile.PRECISION + if time_s < 70.0 or time_s >= 150.0 + else ( + Profile.ROBUST + if time_s < 120.0 + else Profile.HOLDOVER + ) + ), + outlier_period=43, outlier_size_s=900.0e-9, + ), + Scenario( + "startup_noisy_then_clean", 240.0, 30, 1.65, + lambda time_s: 1.1e-6 if time_s < 55.0 else 120.0e-9, + lambda time_s: ( + Profile.ROBUST if time_s < 55.0 else Profile.PRECISION + ), + outlier_period=47, outlier_size_s=1.4e-6, + drift_sigma_mps=0.5, + ), + Scenario( + "noise_bursts_then_recover", 300.0, 30, -2.5, + lambda time_s: ( + 900.0e-9 + if ( + 55.0 <= time_s < 78.0 + or 135.0 <= time_s < 151.0 + or 205.0 <= time_s < 214.0 + ) + else 150.0e-9 + ), + lambda time_s: ( + Profile.ROBUST + if ( + 55.0 <= time_s < 78.0 + or 135.0 <= time_s < 151.0 + or 205.0 <= time_s < 214.0 + ) + else Profile.PRECISION + ), + outlier_period=53, outlier_size_s=1.2e-6, + ), + Scenario( + "satellite_loss_recovery", 260.0, 30, 1.65, + lambda _time_s: 320.0e-9, precision, + invalid_intervals=((55.0, 78.0), (145.0, 151.0)), + ), + Scenario( + "doppler_loss_recovery", 240.0, 30, 1.65, + lambda _time_s: 320.0e-9, precision, + drift_valid_profile=lambda time_s: not ( + 65.0 <= time_s < 105.0 + ), + ), + Scenario( + "reference_range_rate", 180.0, 30, 1.65, + lambda _time_s: 320.0e-9, precision, + reference_range_rate=lambda _time_s: 350.0, + ), + Scenario( + "reference_switch", 220.0, 30, 1.65, + lambda _time_s: 320.0e-9, precision, + reference_range_rate=lambda time_s: ( + 180.0 if time_s < 70.0 else -90.0 + ), + reference_id=lambda time_s: 3 if time_s < 70.0 else 8, + reference_datum_offset=lambda time_s: ( + 0.0 if time_s < 70.0 else 38000.0 + ), + ), + Scenario( + "multipath_step", 240.0, 30, 1.65, + lambda _time_s: 320.0e-9, precision, + timing_step=lambda time_s: ( + 300.0e-9 if 70.0 <= time_s < 120.0 else 0.0 + ), + ), + Scenario( + "no_doppler_drift", 180.0, 30, 1.65, + lambda _time_s: 320.0e-9, precision, + drift_valid=False, + ), + Scenario( + "near_slew_limit", 240.0, 30, 18.0, + lambda _time_s: 320.0e-9, precision, + ), + Scenario( + "beyond_slew_limit", 120.0, 10, 25.0, + lambda _time_s: 50.0e-9, precision, + ), + ) + + +def validate(results: list[ScenarioResult]) -> None: + by_name = {result.name: result for result in results} + for name in ( + "clean_precision", + "board_noise_outliers", + "weak_geometry_robust", + "short_dropouts", + "long_wls_gap", + "quality_transitions", + "startup_noisy_then_clean", + "noise_bursts_then_recover", + "satellite_loss_recovery", + "doppler_loss_recovery", + "reference_range_rate", + "reference_switch", + "multipath_step", + "no_doppler_drift", + "near_slew_limit", + ): + result = by_name[name] + if result.lock_success != result.runs: + raise AssertionError(f"{name}: not every run acquired lock") + if by_name["clean_precision"].first_lock_p95_s >= 15.0: + raise AssertionError("clean acquisition exceeded 15 s") + if by_name["board_noise_outliers"].edge_jitter_p95_ns >= 20.0: + raise AssertionError("board-noise p95 edge jitter exceeded 20 ns") + if by_name["near_slew_limit"].settle_p95_s >= 120.0: + raise AssertionError("near-limit oscillator did not settle in 120 s") + if by_name["beyond_slew_limit"].lock_success != 0: + raise AssertionError("out-of-range oscillator falsely claimed lock") + + +def main() -> int: + results = [run_scenario(scenario) for scenario in scenario_matrix()] + print( + "scenario lock lock_p95 settle_p95 " + "phase_rms edge_jit_med edge_jit_p95 unlock coarse reject" + ) + for result in results: + print( + f"{result.name:27s} " + f"{result.lock_success:2d}/{result.runs:<2d} " + f"{result.first_lock_p95_s:8.2f}s " + f"{result.settle_p95_s:8.2f}s " + f"{result.phase_rms_median_ns:8.2f}ns " + f"{result.edge_jitter_median_ns:8.2f}ns " + f"{result.edge_jitter_p95_ns:8.2f}ns " + f"{result.unlock_total:6d} " + f"{result.coarse_total:6d} " + f"{result.rejected_total:6d}" + ) + validate(results) + print("PPS scenario matrix passed") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/soc/software/pps_python/pps_model.py b/soc/software/pps_python/pps_model.py index 127063e..6e6f79d 100644 --- a/soc/software/pps_python/pps_model.py +++ b/soc/software/pps_python/pps_model.py @@ -23,9 +23,9 @@ import numpy as np C = 299_792_458.0 -RF_HZ = 16_368_000.0 +RF_HZ = 16_367_981.0 TRACKING_HZ = 125_000_000.0 -SAMPLES_PER_CODE = 16_368.0 # samples per millisecond in the C code +SAMPLES_PER_CODE = 16_367.981 # physical RF samples/ms used by the C code CODE_FREQ_BASIS = 1.023e6 START_OFFSET_MS = 68.802 START_OFFSET_S = START_OFFSET_MS / 1000.0 @@ -139,6 +139,7 @@ class EpochResult: transmit_true_s: float reference_arrival_true_s: float snapshot_ready_true_s: float + reference_tracking_tick_raw: int reference_tracking_tick: float reference_abs_sample: float reference_svid: int @@ -642,10 +643,12 @@ class ReceiverEpochSimulator: tracking_tick_continuous = float(self.tracking_clock.ticks_at(raw_event_true_s)) tracking_tick_raw = int(np.floor(tracking_tick_continuous)) # The software correction refers to the code boundary before the raw - # RF edge. Apply the same time displacement to the tracking timestamp. - tracking_tick_corrected = tracking_tick_raw - ( - rem / CODE_FREQ_BASIS - ) * (TRACKING_HZ / RF_HZ) + # RF edge. The mailbox accepts an integer 125 MHz epoch counter, so + # mirror the C implementation's nearest-tick rounding. + capture_correction_ticks = round( + (rem / CODE_FREQ_BASIS) * (TRACKING_HZ / RF_HZ) + ) + tracking_tick_corrected = tracking_tick_raw - capture_correction_ticks return ChannelMeasurement( satellite=satellite, satellite_position_m=satellite.ecef_m.copy(), @@ -725,6 +728,7 @@ class ReceiverEpochSimulator: transmit_true_s=transmit_true_s, reference_arrival_true_s=reference.arrival_true_s, snapshot_ready_true_s=snapshot_ready, + reference_tracking_tick_raw=reference.tracking_tick_raw, reference_tracking_tick=reference.tracking_tick_corrected, reference_abs_sample=reference.corrected_abs_unwrapped, reference_svid=reference.satellite.svid, diff --git a/soc/software/pps_python/simulate_adaptive_pps.py b/soc/software/pps_python/simulate_adaptive_pps.py new file mode 100644 index 0000000..0d24e0d --- /dev/null +++ b/soc/software/pps_python/simulate_adaptive_pps.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python3 +"""Monte-Carlo sweep for noisy/missing WLS adaptive PPS scenarios.""" + +from __future__ import annotations + +import math +import random +import statistics + +try: + from .adaptive_pps import Observation, Profile, QualityMetrics, run_observations +except ImportError: + from adaptive_pps import Observation, Profile, QualityMetrics, run_observations + + +def metrics(profile: Profile) -> QualityMetrics: + if profile == Profile.PRECISION: + return QualityMetrics(1.7, 3.2, 18.0, 38.0, 27.0, 0.35, 0.25, 7) + if profile == Profile.ROBUST: + return QualityMetrics(5.0, 8.0, 65.0, 27.0, 18.0, 0.85, 1.8, 5) + return QualityMetrics(12.0, 18.0, 180.0, 18.0, 10.0, 2.0, 4.0, 4) + + +def scenario(seed: int, duration_s: float = 180.0) -> list[Observation]: + rng = random.Random(seed) + result = [] + for update in range(round(duration_s / 0.2)): + time_s = update * 0.2 + if 70.0 <= time_s < 82.0: + continue + profile = Profile.PRECISION + if 35.0 <= time_s < 45.0 or 110.0 <= time_s < 125.0: + profile = Profile.ROBUST + noise_sigma = 320.0e-9 if profile == Profile.PRECISION else 650.0e-9 + noise_s = rng.gauss(0.0, noise_sigma) + if update % 41 == 0: + noise_s += rng.choice((-1.0, 1.0)) * 900.0e-9 + result.append( + Observation( + time_s, + noise_s, + rng.gauss(0.0, 0.3), + metrics(profile), + ) + ) + return result + + +def summarize(name: str, traces) -> None: + first_locks = [ + trace.first_lock_time() + for trace in traces + if trace.first_lock_time() is not None + ] + tail_rms_ns = [] + jitter_rms_ns = [] + for trace in traces: + tail = trace.phase_error_s[len(trace.phase_error_s) // 2 :] + if tail: + tail_rms_ns.append( + math.sqrt(sum(value * value for value in tail) / len(tail)) + * 1.0e9 + ) + jitter = trace.one_second_jitter_rms(after_s=90.0) + if math.isfinite(jitter): + jitter_rms_ns.append(jitter * 1.0e9) + first_lock_p95 = ( + statistics.quantiles(first_locks, n=20)[18] + if len(first_locks) >= 2 + else (first_locks[0] if first_locks else math.nan) + ) + tail_rms_median = ( + statistics.median(tail_rms_ns) if tail_rms_ns else math.nan + ) + jitter_rms_median = ( + statistics.median(jitter_rms_ns) if jitter_rms_ns else math.nan + ) + print( + f"{name}: lock_success={len(first_locks)}/{len(traces)} " + f"first_lock_p95={first_lock_p95:.2f}s " + f"tail_rms_median={tail_rms_median:.2f}ns " + f"one_second_jitter_median={jitter_rms_median:.2f}ns " + f"unlock_total={sum(trace.unlock_events for trace in traces)}" + ) + + +def main() -> int: + seeds = range(1, 51) + raw = [ + run_observations( + scenario(seed), + finish_time_s=180.0, + adaptive=False, + software_filter=False, + ) + for seed in seeds + ] + adaptive = [ + run_observations(scenario(seed), finish_time_s=180.0) + for seed in seeds + ] + summarize("raw fixed PI", raw) + summarize("adaptive fixed-point PPS", adaptive) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/soc/software/pps_python/test_pps_adaptive.py b/soc/software/pps_python/test_pps_adaptive.py new file mode 100644 index 0000000..9b6ad07 --- /dev/null +++ b/soc/software/pps_python/test_pps_adaptive.py @@ -0,0 +1,322 @@ +from __future__ import annotations + +import math +import random +import unittest + +from pps_python.adaptive_pps import ( + ACQUIRE_PROFILE, + Observation, + Profile, + PROFILES, + QualityMetrics, + QualitySelector, + run_observations, +) + + +def quality(profile: Profile) -> QualityMetrics: + if profile == Profile.PRECISION: + return QualityMetrics( + 1.7, 3.2, 18.0, 38.0, 27.0, 0.35, 0.25, 7 + ) + if profile == Profile.ROBUST: + return QualityMetrics( + 5.0, 8.0, 65.0, 27.0, 18.0, 0.85, 1.8, 5 + ) + return QualityMetrics( + 12.0, 18.0, 180.0, 18.0, 10.0, 2.0, 4.0, 4 + ) + + +def noisy_observations( + duration_s: float, + *, + seed: int, + missing: tuple[float, float] | None = None, + quality_profile: Profile = Profile.PRECISION, +): + rng = random.Random(seed) + observations = [] + update = 0 + while update * 0.2 < duration_s: + time_s = update * 0.2 + update += 1 + if missing is not None and missing[0] <= time_s < missing[1]: + continue + noise_s = rng.gauss(0.0, 320.0e-9) + if update % 37 == 0: + noise_s += rng.choice((-1.0, 1.0)) * 900.0e-9 + observations.append( + Observation( + time_s, + noise_s, + rng.gauss(0.0, 0.25), + quality(quality_profile), + ) + ) + return observations + + +class AdaptivePpsTests(unittest.TestCase): + def test_quality_hysteresis_rejects_metric_chatter(self): + selector = QualitySelector() + for _ in range(7): + self.assertEqual(selector.update(quality(Profile.PRECISION))[0], + Profile.HOLDOVER) + self.assertEqual(selector.update(quality(Profile.PRECISION))[0], + Profile.PRECISION) + + # A one-epoch DOP/CN0 jump must not switch coefficients. + self.assertEqual(selector.update(quality(Profile.ROBUST))[0], + Profile.PRECISION) + self.assertEqual(selector.update(quality(Profile.PRECISION))[0], + Profile.PRECISION) + + for _ in range(2): + self.assertEqual(selector.update(quality(Profile.ROBUST))[0], + Profile.PRECISION) + self.assertEqual(selector.update(quality(Profile.ROBUST))[0], + Profile.ROBUST) + + invalid = quality(Profile.PRECISION) + invalid = QualityMetrics(**{ + **invalid.__dict__, "solution_valid": False + }) + for _ in range(2): + self.assertEqual(selector.update(invalid)[0], Profile.ROBUST) + self.assertEqual(selector.update(invalid)[0], Profile.HOLDOVER) + + def test_unfiltered_board_noise_does_not_meet_lock_contract(self): + observations = noisy_observations(90.0, seed=17) + trace = run_observations( + observations, + finish_time_s=90.0, + adaptive=False, + software_filter=False, + ) + self.assertLess(trace.locked_fraction(), 0.25) + self.assertIsNone(trace.first_lock_time()) + + def test_adaptive_filters_lock_noisy_wls(self): + observations = noisy_observations(120.0, seed=17) + trace = run_observations(observations, finish_time_s=120.0) + first_lock = trace.first_lock_time() + self.assertIsNotNone(first_lock) + self.assertLess(first_lock, 20.0) + tail = trace.phase_error_s[len(trace.phase_error_s) // 2 :] + rms = math.sqrt(sum(value * value for value in tail) / len(tail)) + self.assertLess(rms, 120.0e-9) + self.assertLess( + trace.one_second_jitter_rms(after_s=60.0), + 20.0e-9, + ) + self.assertGreater(trace.locked_fraction(), 0.80) + self.assertLessEqual(trace.unlock_events, 1) + + def test_bounded_wls_gap_holds_then_resumes_without_coarse_step(self): + observations = noisy_observations( + 100.0, seed=23, missing=(35.0, 50.0) + ) + trace = run_observations(observations, finish_time_s=100.0) + baseline = run_observations( + noisy_observations(100.0, seed=23), + finish_time_s=100.0, + ) + before_gap = [ + locked for time_s, locked in zip(trace.time_s, trace.locked) + if 30.0 <= time_s < 35.0 + ] + self.assertTrue(before_gap and all(before_gap)) + # The two-second watchdog suppresses PPS validity during the gap, but + # a bounded outage with the same datum retains the Doppler/PI states. + # Recovery therefore needs no extra coarse commit or phase step. + just_after_gap = [ + locked for time_s, locked in zip(trace.time_s, trace.locked) + if 50.0 <= time_s < 51.0 + ] + self.assertTrue(just_after_gap and all(just_after_gap)) + late = [ + locked for time_s, locked in zip(trace.time_s, trace.locked) + if time_s >= 70.0 + ] + self.assertTrue(late and sum(late) / len(late) > 0.9) + self.assertEqual(trace.coarse_commits, baseline.coarse_commits) + + def test_very_long_wls_gap_reanchors_coarsely(self): + observations = noisy_observations( + 130.0, seed=29, missing=(35.0, 70.0) + ) + trace = run_observations(observations, finish_time_s=130.0) + after_gap = [ + locked for time_s, locked in zip(trace.time_s, trace.locked) + if 70.0 <= time_s < 75.0 + ] + late = [ + locked for time_s, locked in zip(trace.time_s, trace.locked) + if time_s >= 90.0 + ] + self.assertTrue(after_gap and not any(after_gap)) + self.assertTrue(late and sum(late) / len(late) > 0.9) + self.assertGreaterEqual(trace.coarse_commits, 2) + + def test_no_wls_never_claims_lock(self): + trace = run_observations([], finish_time_s=60.0) + self.assertIsNone(trace.first_lock_time()) + self.assertEqual(trace.lock_events, 0) + + def test_satellite_loss_invalidates_then_resumes_predicted_state(self): + observations = noisy_observations(110.0, seed=41) + invalid_quality = QualityMetrics( + **{ + **quality(Profile.PRECISION).__dict__, + "solution_valid": False, + "used_satellites": 2, + "velocity_valid": False, + } + ) + observations = [ + Observation( + item.time_s, + item.timing_error_s, + item.clock_drift_error_mps, + ( + invalid_quality + if 40.0 <= item.time_s < 55.0 + else item.quality + ), + item.clock_drift_valid, + ) + for item in observations + ] + trace = run_observations(observations, finish_time_s=110.0) + baseline = run_observations( + noisy_observations(110.0, seed=41), + finish_time_s=110.0, + ) + during_loss = [ + valid + for time_s, valid in zip(trace.time_s, trace.locked) + if 42.2 <= time_s < 55.0 + ] + after_recovery = [ + valid + for time_s, valid in zip(trace.time_s, trace.locked) + if time_s >= 75.0 + ] + self.assertTrue(during_loss and not any(during_loss)) + self.assertTrue(after_recovery and sum(after_recovery) / len(after_recovery) > 0.9) + self.assertEqual(trace.coarse_commits, baseline.coarse_commits) + + def test_wls_bias_drift_is_negative_reference_range_rate(self): + observations = [ + Observation( + item.time_s, + item.timing_error_s, + item.clock_drift_error_mps, + item.quality, + item.clock_drift_valid, + 350.0, + ) + for item in noisy_observations(120.0, seed=53) + ] + trace = run_observations(observations, finish_time_s=120.0) + self.assertIsNotNone(trace.first_lock_time()) + self.assertLess(trace.first_lock_time(), 20.0) + self.assertLess( + trace.one_second_jitter_rms(after_s=60.0), + 20.0e-9, + ) + + def test_reference_switch_reanchors_instead_of_rejecting_datum_jump(self): + observations = [ + Observation( + item.time_s, + item.timing_error_s, + item.clock_drift_error_mps, + item.quality, + item.clock_drift_valid, + 180.0 if item.time_s < 50.0 else -90.0, + 3 if item.time_s < 50.0 else 8, + 0.0 if item.time_s < 50.0 else 38000.0, + ) + for item in noisy_observations(120.0, seed=59) + ] + trace = run_observations(observations, finish_time_s=120.0) + just_after_switch = [ + locked + for time_s, locked in zip(trace.time_s, trace.locked) + if 50.0 <= time_s < 54.0 + ] + recovered = [ + locked + for time_s, locked in zip(trace.time_s, trace.locked) + if time_s >= 75.0 + ] + self.assertTrue(just_after_switch and not any(just_after_switch)) + self.assertTrue(recovered and sum(recovered) / len(recovered) > 0.9) + self.assertGreaterEqual(trace.coarse_commits, 2) + self.assertEqual(trace.rejected_observations, 0) + + def test_isolated_invalid_wls_does_not_reset_loop(self): + observations = noisy_observations(100.0, seed=61) + baseline = run_observations(observations, finish_time_s=100.0) + invalid = QualityMetrics(**{ + **quality(Profile.PRECISION).__dict__, + "solution_valid": False, + "used_satellites": 2, + "velocity_valid": False, + }) + observations = [ + Observation( + item.time_s, + item.timing_error_s, + item.clock_drift_error_mps, + invalid if abs(item.time_s - 50.0) < 1.0e-9 else item.quality, + item.clock_drift_valid, + ) + for item in observations + ] + trace = run_observations(observations, finish_time_s=100.0) + around_reject = [ + locked + for time_s, locked in zip(trace.time_s, trace.locked) + if 49.0 <= time_s <= 52.0 and abs(time_s - 50.0) > 1.0e-9 + ] + self.assertTrue(around_reject and all(around_reject)) + self.assertEqual(trace.coarse_commits, baseline.coarse_commits) + + def test_receiver_clock_drift_is_not_part_of_relative_wls_datum(self): + observations = noisy_observations(80.0, seed=67) + slow_clock = run_observations( + observations, + finish_time_s=80.0, + receiver_clock_drift_mps=0.0, + ) + fast_clock = run_observations( + observations, + finish_time_s=80.0, + receiver_clock_drift_mps=900.0, + ) + self.assertEqual(slow_clock.phase_error_s, fast_clock.phase_error_s) + self.assertEqual( + slow_clock.frequency_command_ppm, + fast_clock.frequency_command_ppm, + ) + + def test_fixed_point_profiles_fit_register_widths(self): + for profile in PROFILES.values(): + kp, ki, slew = profile.fixed_coefficients(0.2) + self.assertLess(abs(kp), 1 << 47) + self.assertLess(abs(ki), 1 << 47) + self.assertLess(slew, 1 << 32) + self.assertLessEqual(profile.lock_release_ticks, 255) + self.assertLessEqual(profile.lock_acquire_updates, 255) + kp, ki, slew = ACQUIRE_PROFILE.fixed_coefficients(0.2) + self.assertLess(abs(kp), 1 << 47) + self.assertLess(abs(ki), 1 << 47) + self.assertLess(slew, 1 << 32) + + +if __name__ == "__main__": + unittest.main() diff --git a/soc/software/pps_python/test_pps_model.py b/soc/software/pps_python/test_pps_model.py index 4aa7c68..e932a07 100644 --- a/soc/software/pps_python/test_pps_model.py +++ b/soc/software/pps_python/test_pps_model.py @@ -79,6 +79,27 @@ class PpsModelTests(unittest.TestCase): 1000.0 * np.median(np.abs(timing_error)), ) + def test_fractional_code_phase_is_applied_to_capture_counter(self): + _, _, epochs = make_simulation(70.0) + mature = [epoch for epoch in epochs if epoch.transmit_true_s >= 50.0] + applied_s = np.asarray( + [ + ( + epoch.reference_tracking_tick_raw + - epoch.reference_tracking_tick + ) + / TRACKING_HZ + for epoch in mature + ] + ) + self.assertGreater(np.max(applied_s), 40.0e-9) + self.assertTrue(np.all(applied_s >= 0.0)) + self.assertTrue(np.all(applied_s <= 64.0e-9)) + # The integer mailbox can represent the correction only on the 8 ns + # PPS grid; the model must use the same nearest-tick quantization. + applied_ticks = applied_s * TRACKING_HZ + self.assertLess(np.max(np.abs(applied_ticks - np.rint(applied_ticks))), 1.0e-9) + def test_timestamped_pi_rejects_apb_latency_as_phase(self): _, tracking, epochs = make_simulation(70.0) config = PIConfig(natural_frequency_hz=0.035, damping=0.9, max_slew_ppm=20.0) diff --git a/targets/fpga/genesys2/Makefile b/targets/fpga/genesys2/Makefile index b5daf37..59a0e2c 100644 --- a/targets/fpga/genesys2/Makefile +++ b/targets/fpga/genesys2/Makefile @@ -68,7 +68,7 @@ FLASH_ERASE ?= 1 FLASH_REGIONS ?= auto FLASH_FORCE ?= 0 GPS_DEBUG ?= 1 -GPS_DEBUG_LEVEL ?= 4 +GPS_DEBUG_LEVEL ?= 1 FLASH_MCS ?= $(OUT_DIR)/flash/genesys2_rocket.mcs FLASH_BIN ?= $(OUT_DIR)/flash/genesys2_rocket.bin