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

feat(pps): add adaptive disciplined timing loop

Browse Source
This commit is contained in:
onur
2026-07-25 17:56:41 +03:00
parent 0f6b3482c5
commit ecbb8d0b68
39 changed files with 4505 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
hardware/external/gps/rtl/pps/README.md
Vendored
+49 -11
View File
@@ -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.
hardware/external/gps/rtl/pps/pps_core.sv
Vendored
+23 -7
View File
@@ -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
hardware/external/gps/rtl/pps/pps_pkg.sv
Vendored
+9 -2
View File
@@ -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;
hardware/external/gps/rtl/pps/pps_wrapper.sv
Vendored
+14 -2
View File
@@ -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),
hardware/external/gps/rtl/pps/tb/Makefile
Vendored
+1
View File
@@ -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:
hardware/external/gps/rtl/pps/tb/pps_adaptive_tb.sv
Vendored
+191
View File
@@ -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
hardware/external/gps/rtl/pps/tb/pps_convergence_tb.sv
Vendored
+1
View File
@@ -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),
hardware/external/gps/rtl/pps/tb/pps_core_tb.sv
Vendored
+1
View File
@@ -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),
hardware/external/gps/rtl/pps/tb/pps_wrapper_tb.sv
Vendored
+33 -2
View File
@@ -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);
hardware/external/gps/rtl/pps/tb/run.do
Vendored
+1
View File
@@ -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
soc/software/Makefile
+1 -1
View File
@@ -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
soc/software/gps_system/Makefile
+6 -2
View File
@@ -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)
soc/software/gps_system/README.md
+99 -2
View File
@@ -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.
soc/software/gps_system/header/gps/NavEquations.h
+1
View File
@@ -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;
soc/software/gps_system/header/gps/Navigation.h
+15 -2
View File
@@ -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
soc/software/gps_system/header/gps/gps_pps.h
+30 -1
View File
@@ -4,11 +4,40 @@
#include <stdbool.h>
#include <stdint.h>
#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
soc/software/gps_system/header/gps/gps_pps_quality.h
+62
View File
@@ -0,0 +1,62 @@
#ifndef GPS_PPS_QUALITY_H
#define GPS_PPS_QUALITY_H
#include <stdbool.h>
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
soc/software/gps_system/header/gps/gps_pps_time_filter.h
+82
View File
@@ -0,0 +1,82 @@
#ifndef GPS_PPS_TIME_FILTER_H
#define GPS_PPS_TIME_FILTER_H
#include <stdbool.h>
#include <stdint.h>
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
soc/software/gps_system/header/gps/gps_time_anchor.h
+34
View File
@@ -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. */
soc/software/gps_system/src/gps/NavEquations.c
+22
View File
@@ -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);
soc/software/gps_system/src/gps/gps_interrupt_handlers.c
+8 -3
View File
@@ -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,14 +1389,17 @@ 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,
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,
SAMPLES_PER_CODE * 1000.0,
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))
@@ -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();
soc/software/gps_system/src/gps/gps_ip.c
+271 -18
View File
@@ -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 <bsp/irq.h>
@@ -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)
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))
{
minimum = psTVLtime[i];
minimum_index = i;
}
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;
soc/software/gps_system/src/gps/gps_pps.c
+200 -11
View File
@@ -1,4 +1,5 @@
#include "gps/gps_pps.h"
#include "gps/gps_macros.h"
#include <bsp/soc.h>
#include <math.h>
@@ -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_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;
}
soc/software/gps_system/src/gps/gps_pps_quality.c
+194
View File
@@ -0,0 +1,194 @@
#include "gps/gps_pps_quality.h"
#include <limits.h>
#include <math.h>
#include <stddef.h>
#include <string.h>
#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";
}
}
soc/software/gps_system/src/gps/gps_pps_time_filter.c
+309
View File
@@ -0,0 +1,309 @@
#include "gps/gps_pps_time_filter.h"
#include <math.h>
#include <stddef.h>
#include <string.h>
/*
* 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;
}
soc/software/gps_system/src/gps/gps_time_anchor.c
+103
View File
@@ -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,
soc/software/gps_system/tests/Makefile
+52 -1
View File
@@ -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
soc/software/gps_system/tests/test_gps_pps_adaptive_loop.c
+542
View File
@@ -0,0 +1,542 @@
#include <assert.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#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;
}
soc/software/gps_system/tests/test_gps_pps_quality.c
+173
View File
@@ -0,0 +1,173 @@
#include <assert.h>
#include <math.h>
#include <stdio.h>
#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;
}
soc/software/gps_system/tests/test_gps_pps_time_filter.c
+215
View File
@@ -0,0 +1,215 @@
#include <assert.h>
#include <math.h>
#include <stdio.h>
#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;
}
soc/software/gps_system/tests/test_gps_time_anchor.c
+88 -1
View File
@@ -1,10 +1,11 @@
#include "gps/gps_time_anchor.h"
#include "gps/Navigation.h"
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#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");
soc/software/pps_python/README.md
+78 -12
View File
@@ -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
soc/software/pps_python/adaptive_pps.py
+718
View File
@@ -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)
soc/software/pps_python/analyze_pps_scenarios.py
+406
View File
@@ -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())
soc/software/pps_python/pps_model.py
+10 -6
View File
@@ -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,
soc/software/pps_python/simulate_adaptive_pps.py
+108
View File
@@ -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())
soc/software/pps_python/test_pps_adaptive.py
+322
View File
@@ -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()
soc/software/pps_python/test_pps_model.py
+21
View File
@@ -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)
targets/fpga/genesys2/Makefile
+1 -1
View File
@@ -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