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

fix(gps): correct week-number handling across almanac, ephemeris and flash

Browse Source 
Persisting navigation data changed its lifetime from one power cycle to
weeks, and every piece of arithmetic that had only ever run over the
shorter lifetime turned out to be wrong. Each fix below reproduces in a
host test that fails against the previous behaviour.

Almanac (gps_almanac.c, gps_lnav_parser.c)

WNa was lifted against gps_ephems.toe.week, a field nothing ever writes,
so any WNa above 128 underflowed to ~4.29e9. The propagation then reduced
tk with a while-loop, turning one visibility refresh into 4.29e9
iterations - hours on the target, inside a 10 s periodic call. WNa is the
week modulo 256, so this armed at GPS week 2433 and then for 127 weeks in
every 256. Now lifted against tow.week in signed arithmetic, refused if
impossible, and the propagation is closed form.

The IS-GPS-200 20.3.3.5.2.3 half-week wrap was also applied to tk
unconditionally. It is a week-crossover correction, not a general
reduction: a GPS week is 14.0384 orbital periods, so aliasing one week
moves a satellite 5161 km, enough to report one 5.1 degrees below the
horizon as 5.7 above. The crossover guess now lives in
gps_almanac_assumed_week(), used only before page 25 arrives, and an
almanac too old to propagate yields no prediction rather than a wrong one.

Ephemeris age (gps_ephemeris_validity.c, gps_lnav_parser.c)

The curve-fit check compared two second-of-week values and never read a
week at all, so a record exactly N weeks old read as current. Harmless
while ephemeris lived in RAM; directly reachable once persisted across a
power-off. gps_resolve_toe_week() now reconstructs Toe's week at commit
time from the subframe 1 week number and the handover word; an
unresolvable week yields no age, which reads as a rejection.

A restored record could also outrank the live broadcast, because the Toe
ordering inverts past half a week. Flash records are now marked restored
and are candidates, never incumbents.

Flash wear (gps_nav_context_store.c, gps_interrupt_handlers.c)

The CRC wear policy never engaged: the payload carried the live WLS
position and a receiver timestamp nothing on the restore path read, so
every 60 s save was an erase - about 139 days to wear-out. The seed is
now held and refreshed only on 5 km of movement, at most hourly: one
erase per day instead of 1440.

Measurement model (NavEquations.c)

The elevation weighting was floored at 10 degrees, capping its term at
17.3 m where the model's own answer is 34.4 m at 5 degrees and 86 m at 2,
so a low satellite was trusted several times more than the model allowed.
Floor lowered to 3 degrees, which is where 1/sin must still be bounded
because a zero weight fails the whole epoch. Measured on a six-satellite
geometry, 30 m of uncorrected delay now moves the fix 4.18 m through a
4-degree satellite against 48.50 m through a 72-degree one.

Both atmospheric corrections switched off below the horizon, so a
satellite crossing it lost 78.8 m of tropospheric delay in one step.
Elevation is now clamped at the horizon for the models, and the
extrapolation is paid for in variance.

Also: gps_pps_select_reference() returned early on the preferred PRN,
leaving later non-finite capture times unscreened.

Checked and found correct, with regressions added: the epoch assembly
under satellite connect/disconnect and reassignment, coalesced captures,
the 32-bit RF sample counter wrap (pseudoranges agree to 3.3 um across
it), the refusal to publish a solvable subset of an incoherent epoch, and
the Klobuchar model against IS-GPS-200 20.3.3.5.2.5.

Tests: 22 host suites, 5 RTL testbenches, sanitizer clean.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
onur
2026-08-10 01:45:35 +03:00
parent 471ff12e70
commit f59d6ebb43
49 changed files with 12098 additions and 478 deletions
Download Patch File Download Diff File Expand all files Collapse all files
hardware/external/gps/rtl/apb_wrappers/tracking_wrapper.sv
Vendored
+33 -2
View File
@@ -67,6 +67,8 @@ module tracking_wrapper #(
// nav solve timer I/O
logic nav_solve_timer_reached;
logic nav_solve_timer_load_abs_sample_signal;
logic nav_solve_phase_seed_consumed;
logic nav_solve_provisional;
//
@@ -117,6 +119,7 @@ module tracking_wrapper #(
logic [31 : 0] control_reg_p, control_reg_n;
logic [31 : 0] timeout_reg_p, timeout_reg_n;
logic [31 : 0] nav_solve_timer_reg_p, nav_solve_timer_reg_n;
logic [31 : 0] nav_solve_phase_seed_reg_p, nav_solve_phase_seed_reg_n;
logic [30 : 0] nav_bit_read_reg_p[10], nav_bit_read_reg_n[10];
// Atomic APB-domain copy of the 20 ms measurement.
@@ -363,6 +366,7 @@ module tracking_wrapper #(
intr_pending_p <= '0;
timeout_reg_p <= trck_pkg::TRCK_TIMEOUT_RESET_VAL;
nav_solve_timer_reg_p <= 'ha;
nav_solve_phase_seed_reg_p <= '0;
timeout_counter_p <= '0;
start_trck_ctl_p <= '0;
stop_trck_ctl_p <= '0;
@@ -395,6 +399,7 @@ module tracking_wrapper #(
intr_pending_p <= intr_pending_n;
timeout_reg_p <= timeout_reg_n;
nav_solve_timer_reg_p <= nav_solve_timer_reg_n;
nav_solve_phase_seed_reg_p <= nav_solve_phase_seed_reg_n;
timeout_counter_p <= timeout_counter_n;
start_trck_ctl_p <= start_trck_ctl_n;
stop_trck_ctl_p <= stop_trck_ctl_n;
@@ -635,7 +640,11 @@ module tracking_wrapper #(
.subframe_ready(load_sigs), // signal that asserts 1 clock cycle after a subframe bit reached ( in essence, 300 bit + 8 preamble bit !!!)
.subframe_count(how_z_count), // HOW Z-count of this globally identified subframe boundary.
.trck_busy(busy_syned), // stop counting when tracking is unlocked or do not start to count anyways.
.phase_seed_arm(control_reg_p[TRCK_NAV_SOLVE_SEED_ARM_POS]), // firmware warm-start phase estimate is pending.
.phase_seed_value(nav_solve_phase_seed_reg_p), // navigation-bit phase, already reduced modulo counter_max_val.
// output from module.
.phase_seed_consumed(nav_solve_phase_seed_consumed), // timer answered the seed; clears the arm bit.
.nav_solve_provisional(nav_solve_provisional), // epochs are running on an unconfirmed seed.
.nav_solve_timer_reached(nav_solve_timer_reached), // timer internal count is equal to predefined count value. (status flag) asserted 1 clock cycle. INTERRUPT SIGNAL TO CPU FROM TIMER TODO: will be integrated to main interrupt generation mechanishm.
.load_abs_sample(nav_solve_timer_load_abs_sample_signal) // when counter reaches to predefined value, trigger 1 clock cycle wide abs. sample load signal.
);
@@ -656,6 +665,7 @@ module tracking_wrapper #(
control_reg_n = control_reg_p;
timeout_reg_n = timeout_reg_p;
nav_solve_timer_reg_n = nav_solve_timer_reg_p;
nav_solve_phase_seed_reg_n = nav_solve_phase_seed_reg_p;
if(apb_bus_ps == APB_STATE_SEL) begin
if(PWRITE) begin // write to register
@@ -691,6 +701,8 @@ module tracking_wrapper #(
if(!PWDATA[TRCK_MEASUREMENT_INTR_EN_POS]) control_reg_n[TRCK_MEASUREMENT_INTR_EN_POS] = 1'b0;
if(PWDATA[TRCK_MEASUREMENT_FLAG_EN_POS]) control_reg_n[TRCK_MEASUREMENT_FLAG_EN_POS] = 1'b1;
if(!PWDATA[TRCK_MEASUREMENT_FLAG_EN_POS]) control_reg_n[TRCK_MEASUREMENT_FLAG_EN_POS] = 1'b0;
if(PWDATA[TRCK_NAV_SOLVE_SEED_ARM_POS]) control_reg_n[TRCK_NAV_SOLVE_SEED_ARM_POS] = 1'b1;
if(!PWDATA[TRCK_NAV_SOLVE_SEED_ARM_POS]) control_reg_n[TRCK_NAV_SOLVE_SEED_ARM_POS] = 1'b0;
end
(LOW_ADDRESS + PRN_NUMBER_REG_OFFSET):
begin
@@ -715,6 +727,10 @@ module tracking_wrapper #(
begin
nav_solve_timer_reg_n = PWDATA;
end
(LOW_ADDRESS + NAV_SOLVE_PHASE_SEED_REG_OFFSET):
begin
nav_solve_phase_seed_reg_n = PWDATA;
end
default:
@@ -738,7 +754,8 @@ module tracking_wrapper #(
(control_reg_p[TRCK_STOP_ON_UNLOCK_FLAG_EN_POS] << trck_pkg::TRCK_STOP_ON_UNLOCK_FLAG_EN_POS) |
(control_reg_p[TRCK_NAV_SOLVE_FLAG_EN_POS] << trck_pkg::TRCK_NAV_SOLVE_FLAG_EN_POS) |
(control_reg_p[TRCK_MEASUREMENT_INTR_EN_POS] << trck_pkg::TRCK_MEASUREMENT_INTR_EN_POS) |
(control_reg_p[TRCK_MEASUREMENT_FLAG_EN_POS] << trck_pkg::TRCK_MEASUREMENT_FLAG_EN_POS)
(control_reg_p[TRCK_MEASUREMENT_FLAG_EN_POS] << trck_pkg::TRCK_MEASUREMENT_FLAG_EN_POS) |
(control_reg_p[TRCK_NAV_SOLVE_SEED_ARM_POS] << trck_pkg::TRCK_NAV_SOLVE_SEED_ARM_POS)
);
end
(LOW_ADDRESS + STATUS_REG_OFFSET):
@@ -751,7 +768,8 @@ module tracking_wrapper #(
(trck_timeout_flag_p << TRCK_UNLOCK_FLAG_POS) |
(trck_nav_bit_rcv_flag_p << TRCK_NAV_BITS_RCV_FLAG_POS) |
(trck_nav_solve_flag_p << TRCK_NAV_SOLVE_FLAG_POS) |
(trck_measurement_flag_p << TRCK_MEASUREMENT_FLAG_POS)
(trck_measurement_flag_p << TRCK_MEASUREMENT_FLAG_POS) |
(nav_solve_provisional << TRCK_NAV_SOLVE_PROVISIONAL_POS)
);
end
@@ -827,6 +845,10 @@ module tracking_wrapper #(
begin
PRDATA = nav_solve_timer_reg_p;
end
(LOW_ADDRESS + NAV_SOLVE_PHASE_SEED_REG_OFFSET):
begin
PRDATA = nav_solve_phase_seed_reg_p;
end
(LOW_ADDRESS + REM_CODE_PH_REG_OFFSET):
begin
//PRDATA = rem_code_ph_reg_p;
@@ -887,5 +909,14 @@ module tracking_wrapper #(
endcase
end
end
/* The timer answers an armed seed in one cycle and in every state, so
* clearing the arm bit here cannot race an APB write: firmware can only
* observe the bit set on the cycle after its own write, and consumption
* requires the bit to already be registered. Applying the clear after
* the decode keeps it authoritative. */
if(nav_solve_phase_seed_consumed) begin
control_reg_n[TRCK_NAV_SOLVE_SEED_ARM_POS] = 1'b0;
end
end : apb_register_write_read_comb
endmodule
hardware/external/gps/rtl/apb_wrappers/trck_pkg.sv
Vendored
+11 -3
View File
@@ -30,7 +30,8 @@ package trck_pkg;
MEASUREMENT_SUM_I_REG_OFFSET = 100,
MEASUREMENT_SUM_Q_REG_OFFSET = 104,
MEASUREMENT_WBP_LO_REG_OFFSET = 108,
MEASUREMENT_WBP_HI_REG_OFFSET = 112
MEASUREMENT_WBP_HI_REG_OFFSET = 112,
NAV_SOLVE_PHASE_SEED_REG_OFFSET = 116
}trck_wrapper_reg_offets_e;
@@ -50,7 +51,11 @@ package trck_pkg;
TRCK_NAV_SOLVE_INTR_EN_POS = 11, // clear pos
TRCK_NAV_SOLVE_FLAG_EN_POS = 12, // clear pos
TRCK_MEASUREMENT_INTR_EN_POS = 13,
TRCK_MEASUREMENT_FLAG_EN_POS = 14
TRCK_MEASUREMENT_FLAG_EN_POS = 14,
/* Write 1 to arm the NAV_SOLVE_PHASE_SEED value. Hardware clears this
* bit as soon as the timer has answered the seed, so it reads back 0
* once the request has been evaluated. */
TRCK_NAV_SOLVE_SEED_ARM_POS = 15
}trck_control_reg_pos_e;
typedef enum
@@ -62,7 +67,10 @@ package trck_pkg;
TRCK_NAV_BITS_RCV_FLAG_POS = 4,
TRCK_UNLOCK_FLAG_POS = 5,
TRCK_NAV_SOLVE_FLAG_POS = 6,
TRCK_MEASUREMENT_FLAG_POS = 7
TRCK_MEASUREMENT_FLAG_POS = 7,
/* NAV solve epochs are running on a firmware seed that no decoded
* subframe has confirmed yet. */
TRCK_NAV_SOLVE_PROVISIONAL_POS = 8
}trck_status_reg_pos_e;
localparam logic [7 : 0] PREAMBLE = 8'b10001011;
hardware/external/gps/rtl/gps_timer/nav_solve_timer.sv
Vendored
+73 -10
View File
@@ -8,8 +8,18 @@ module nav_solve_timer(
input logic subframe_ready,
input logic [16 : 0] subframe_count,
input logic trck_busy,
/* Warm/hot reacquisition seed. phase_seed_arm is a level held
* by the wrapper control register. The timer always answers it
* with phase_seed_consumed, in every state, so an armed seed has
* a lifetime of one evaluation and can never be applied to a
* later reacquisition. nav_solve_provisional reports whether the
* seed was actually taken. */
input logic phase_seed_arm,
input logic [31 : 0] phase_seed_value,
output logic phase_seed_consumed,
output logic nav_solve_provisional,
output logic nav_solve_timer_reached,
output logic load_abs_sample
output logic load_abs_sample
);
logic [1 : 0] bit_valid_cc_p, bit_valid_cc_n;
@@ -23,7 +33,7 @@ module nav_solve_timer(
assign bit_valid_pulse = (bit_valid_cc_p[0] & !bit_valid_cc_p[1]);
assign load_abs_sample = (abs_sample_load_cc_p[0] & !abs_sample_load_cc_p[1]);
assign bit_valid_cc_n = {bit_valid_cc_p[0], bit_valid};
assign bit_valid_cc_n = {bit_valid_cc_p[0], bit_valid};
assign nav_solve_timer_reached = intr_delay_ff;
@@ -34,8 +44,23 @@ module nav_solve_timer(
logic [4 : 0] phase_bit_p, phase_bit_n;
logic [32 : 0] phase_candidate;
logic [32 : 0] phase_reduced;
/* Set while the epoch phase came from a firmware seed and has not yet been
* confirmed by a decoded subframe. The broadcast HOW Z-count stays the
* authority: the first real subframe re-aligns the phase and clears this. */
logic provisional_p, provisional_n;
logic subframe_align_req;
logic seed_usable;
logic seed_load_now;
nav_solve_timer_pkg::nav_solve_timer_e nav_solve_state_p, nav_solve_state_n;
nav_solve_timer_pkg::nav_solve_timer_e nav_solve_state_p, nav_solve_state_n;
assign nav_solve_provisional = provisional_p;
assign subframe_align_req = subframe_ready && trck_busy && enable &&
(counter_max_val != 0);
/* Firmware must reduce the phase itself; an unreduced seed is dropped rather
* than silently aliased onto the wrong navigation-bit grid. */
assign seed_usable = trck_busy && enable && (counter_max_val != 0) &&
(phase_seed_value < counter_max_val);
always_ff @(posedge clk or negedge resetn) begin : counter_seq_logic
if(!resetn) begin
@@ -44,9 +69,10 @@ module nav_solve_timer(
phase_remainder_p <= '0;
phase_bit_p <= '0;
bit_valid_cc_p <= '0;
abs_sample_load_cc_p <= '0;
nav_solve_state_p <= nav_solve_timer_pkg::IDLE;
abs_sample_load_cc_p <= '0;
nav_solve_state_p <= nav_solve_timer_pkg::IDLE;
intr_delay_ff <= '0;
provisional_p <= '0;
end
else begin
internal_counter_p <= internal_counter_n;
@@ -54,9 +80,10 @@ module nav_solve_timer(
phase_remainder_p <= phase_remainder_n;
phase_bit_p <= phase_bit_n;
bit_valid_cc_p <= bit_valid_cc_n;
abs_sample_load_cc_p <= abs_sample_load_cc_n;
nav_solve_state_p <= nav_solve_state_n;
abs_sample_load_cc_p <= abs_sample_load_cc_n;
nav_solve_state_p <= nav_solve_state_n;
intr_delay_ff <= load_abs_sample;
provisional_p <= provisional_n;
end
end : counter_seq_logic
@@ -69,7 +96,10 @@ module nav_solve_timer(
phase_reduced = '0;
load_abs_sample_internal = '0;
nav_solve_state_n = nav_solve_state_p;
if((nav_solve_state_p == nav_solve_timer_pkg::IDLE) && subframe_ready && trck_busy && enable && (counter_max_val != 0)) begin
provisional_n = provisional_p;
phase_seed_consumed = '0;
seed_load_now = '0;
if((nav_solve_state_p == nav_solve_timer_pkg::IDLE) && subframe_align_req) begin
/* HOW Z-count identifies this boundary globally in six-second
* subframes. Seed the per-channel counter with the corresponding
* navigation-bit phase so channels which lock on different
@@ -84,6 +114,18 @@ module nav_solve_timer(
phase_bit_n = 5'd25;
nav_solve_state_n = nav_solve_timer_pkg::ALIGN;
end
/* Answer an armed seed in every state so it cannot survive to a later
* reacquisition. It is only applied from IDLE, and a real subframe in the
* same cycle always wins. */
if(phase_seed_arm) begin
phase_seed_consumed = '1;
if((nav_solve_state_p == nav_solve_timer_pkg::IDLE) &&
!subframe_align_req && seed_usable) begin
seed_load_now = '1;
end
end
if(nav_solve_state_p == nav_solve_timer_pkg::ALIGN) begin
if(!enable || !trck_busy || (counter_max_val == 0)) begin
nav_solve_state_n = nav_solve_timer_pkg::IDLE;
@@ -97,6 +139,8 @@ module nav_solve_timer(
phase_remainder_n = phase_reduced[31:0];
if(phase_bit_p == 0) begin
internal_counter_n = phase_reduced[31:0];
/* The broadcast phase has replaced any seeded estimate. */
provisional_n = '0;
nav_solve_state_n = nav_solve_timer_pkg::COUNT;
end
else begin
@@ -108,18 +152,37 @@ module nav_solve_timer(
if(!enable) nav_solve_state_n = nav_solve_timer_pkg::IDLE;
if(!trck_busy) nav_solve_state_n = nav_solve_timer_pkg::IDLE;
end
if((nav_solve_state_p == nav_solve_timer_pkg::COUNT) && provisional_p &&
subframe_align_req) begin
/* First decoded subframe after a seeded start. Re-run the authoritative
* alignment so a wrong firmware estimate self-corrects within one
* subframe instead of persisting for the whole lock. */
phase_dividend_n = ({9'b0, subframe_count} << 8) +
({9'b0, subframe_count} << 5) +
({9'b0, subframe_count} << 3) +
({9'b0, subframe_count} << 2);
phase_remainder_n = '0;
phase_bit_n = 5'd25;
nav_solve_state_n = nav_solve_timer_pkg::ALIGN;
end
if(nav_solve_state_p == nav_solve_timer_pkg::COUNT) begin
if(bit_valid_pulse) begin
//internal_counter_n = (internal_counter_p == counter_max_val) ? 0 : (internal_counter_p + 1);
internal_counter_n = (internal_counter_p == (counter_max_val - 1)) ? 0 : (internal_counter_p + 1);
end
end
if(nav_solve_state_p == nav_solve_timer_pkg::IDLE) begin
if((nav_solve_state_p == nav_solve_timer_pkg::IDLE) && !seed_load_now) begin
internal_counter_n = '0;
provisional_n = '0;
end
if(seed_load_now) begin
internal_counter_n = phase_seed_value;
provisional_n = '1;
nav_solve_state_n = nav_solve_timer_pkg::COUNT;
end
if((nav_solve_state_p == nav_solve_timer_pkg::COUNT) && (internal_counter_p == (counter_max_val - 1) && (bit_valid_pulse))) begin
load_abs_sample_internal = '1;
load_abs_sample_internal = '1;
end
end : counter_comb_logic
hardware/external/gps/rtl/gps_timer/tb/nav_solve_timer_seed_tb.sv
Vendored
+436
View File
@@ -0,0 +1,436 @@
`timescale 1ns/1ps
/*
* Warm/hot reacquisition phase seed for the navigation-solve epoch timer.
*
* Epoch definition being checked throughout: with the counter loaded to P at
* some navigation-bit boundary, the interrupt fires on the bit whose
* pre-increment counter value is (COUNTER_MAX-1). The subframe path loads
* P = (Z_count * 300) mod COUNTER_MAX, so epochs land on absolute navigation-bit
* indices b where (b + 1) mod COUNTER_MAX == 0. A correct seed must reproduce
* exactly that grid.
*/
module nav_solve_timer_seed_tb;
localparam int unsigned COUNTER_MAX = 10;
logic clk = 1'b0;
logic resetn = 1'b0;
logic enable = 1'b0;
logic [31:0] counter_max_val = COUNTER_MAX;
logic bit_valid = 1'b0;
logic subframe_ready = 1'b0;
logic [16:0] subframe_count = '0;
logic trck_busy = 1'b0;
logic phase_seed_arm = 1'b0;
logic [31:0] phase_seed_value = '0;
logic phase_seed_consumed;
logic nav_solve_provisional;
logic nav_solve_timer_reached;
logic load_abs_sample;
int unsigned epoch_count;
int unsigned consumed_count;
int unsigned bit_index;
int unsigned last_epoch_bit;
bit last_epoch_bit_valid;
always #4 clk = ~clk;
nav_solve_timer dut (
.clk,
.resetn,
.enable,
.counter_max_val,
.bit_valid,
.subframe_ready,
.subframe_count,
.trck_busy,
.phase_seed_arm,
.phase_seed_value,
.phase_seed_consumed,
.nav_solve_provisional,
.nav_solve_timer_reached,
.load_abs_sample
);
/* The timer detects bit_valid as a synchronised rising edge, so every
* navigation bit is one pulse well separated from the next. */
task automatic nav_bit();
bit_valid = 1'b1;
@(posedge clk);
@(posedge clk);
bit_valid = 1'b0;
repeat(6) @(posedge clk);
bit_index++;
endtask
task automatic nav_bits(input int unsigned count);
for(int unsigned i = 0; i < count; i++) nav_bit();
endtask
/* Assert subframe_ready coincident with a navigation bit, as the wrapper
* does: load_sigs is qualified by the same bit_valid edge. */
task automatic nav_bit_with_subframe(input int unsigned z_count);
subframe_count = 17'(z_count);
bit_valid = 1'b1;
@(posedge clk);
subframe_ready = 1'b1;
@(posedge clk);
subframe_ready = 1'b0;
bit_valid = 1'b0;
repeat(30) @(posedge clk); // let ALIGN finish its 26-cycle reduction
bit_index++;
endtask
task automatic arm_seed(input int unsigned value);
phase_seed_value = 32'(value);
phase_seed_arm = 1'b1;
@(posedge clk);
/* Mirror the wrapper: the arm bit is cleared as soon as hardware answers. */
while(!phase_seed_consumed) @(posedge clk);
@(posedge clk);
phase_seed_arm = 1'b0;
@(posedge clk);
endtask
task automatic reset_dut();
resetn = 1'b0;
enable = 1'b0;
trck_busy = 1'b0;
bit_valid = 1'b0;
subframe_ready = 1'b0;
phase_seed_arm = 1'b0;
phase_seed_value = '0;
epoch_count = 0;
consumed_count = 0;
bit_index = 0;
last_epoch_bit_valid = 1'b0;
repeat(4) @(posedge clk);
resetn = 1'b1;
repeat(4) @(posedge clk);
endtask
always @(posedge clk) begin
if(resetn) begin
if(nav_solve_timer_reached) begin
epoch_count++;
last_epoch_bit = bit_index;
last_epoch_bit_valid = 1'b1;
end
if(phase_seed_consumed) consumed_count++;
end
end
// --------------------------------------------------------------------
// 1. Regression: the subframe path is unchanged by the seed logic.
// --------------------------------------------------------------------
task automatic test_subframe_path_unchanged();
int unsigned z;
int unsigned phase;
int unsigned bits_to_epoch;
reset_dut();
enable = 1'b1;
trck_busy = 1'b1;
z = 7;
phase = (z * 300) % COUNTER_MAX;
nav_bit_with_subframe(z);
if(nav_solve_provisional)
$fatal(1, "subframe alignment must not be provisional");
/* Counter now holds phase; the epoch bit is the one whose pre-increment
* value is COUNTER_MAX-1. */
bits_to_epoch = COUNTER_MAX - 1 - phase;
epoch_count = 0;
nav_bits(bits_to_epoch);
if(epoch_count != 0)
$fatal(1, "epoch fired early: count=%0d", epoch_count);
nav_bit();
repeat(8) @(posedge clk);
if(epoch_count != 1)
$fatal(1, "subframe-aligned epoch missing: count=%0d", epoch_count);
/* And it repeats every COUNTER_MAX bits. */
epoch_count = 0;
nav_bits(COUNTER_MAX);
repeat(8) @(posedge clk);
if(epoch_count != 1)
$fatal(1, "periodic epoch missing: count=%0d", epoch_count);
$display(" [1] subframe path unchanged");
endtask
// --------------------------------------------------------------------
// 2. A valid seed starts epochs with no subframe at all.
// --------------------------------------------------------------------
task automatic test_seed_starts_epochs();
int unsigned phase;
reset_dut();
enable = 1'b1;
trck_busy = 1'b1;
phase = 3;
arm_seed(phase);
if(!nav_solve_provisional)
$fatal(1, "seed was not taken");
if(consumed_count == 0)
$fatal(1, "seed was never answered");
epoch_count = 0;
nav_bits(COUNTER_MAX - 1 - phase);
if(epoch_count != 0)
$fatal(1, "seeded epoch fired early");
nav_bit();
repeat(8) @(posedge clk);
if(epoch_count != 1)
$fatal(1, "seeded epoch missing");
$display(" [2] seed starts epochs without a subframe");
endtask
// --------------------------------------------------------------------
// 3. Both paths place epochs on the same global navigation-bit grid.
//
// This is the contract firmware depends on, so it is checked in absolute
// navigation-bit indices rather than by comparing raw testbench counts.
//
// Seed semantics: phase_seed_value is the counter value in effect for the
// NEXT navigation bit to arrive. The subframe path is identical - the
// Z-count phase (Z*300) mod N applies to the first bit after the subframe
// boundary, whose absolute index is Z*300. So in both cases
//
// seed = (absolute index of the next bit) mod N
//
// and an epoch must land wherever (absolute index + 1) mod N == 0.
// --------------------------------------------------------------------
task automatic test_seed_matches_subframe_grid();
int unsigned z;
int unsigned phase;
int unsigned subframe_epoch_abs;
int unsigned seeded_epoch_abs;
z = 11;
phase = (z * 300) % COUNTER_MAX;
reset_dut();
enable = 1'b1;
trck_busy = 1'b1;
nav_bit_with_subframe(z);
/* The subframe consumed testbench bit 0; the next bit, testbench index 1,
* is absolute index z*300. */
epoch_count = 0;
last_epoch_bit_valid = 1'b0;
nav_bits(2 * COUNTER_MAX);
repeat(8) @(posedge clk);
if(!last_epoch_bit_valid) $fatal(1, "no subframe epoch observed");
subframe_epoch_abs = (z * 300) + last_epoch_bit - 1;
reset_dut();
enable = 1'b1;
trck_busy = 1'b1;
/* No bit is consumed by arming, so the next bit - testbench index 0 - is
* the one the seed describes: absolute index z*300. */
arm_seed(phase);
epoch_count = 0;
last_epoch_bit_valid = 1'b0;
nav_bits(2 * COUNTER_MAX);
repeat(8) @(posedge clk);
if(!last_epoch_bit_valid) $fatal(1, "no seeded epoch observed");
seeded_epoch_abs = (z * 300) + last_epoch_bit;
if(((subframe_epoch_abs + 1) % COUNTER_MAX) != 0)
$fatal(1, "subframe epoch off the global grid: abs=%0d",
subframe_epoch_abs);
if(((seeded_epoch_abs + 1) % COUNTER_MAX) != 0)
$fatal(1, "seeded epoch off the global grid: abs=%0d",
seeded_epoch_abs);
if(subframe_epoch_abs != seeded_epoch_abs)
$fatal(1, "seeded grid differs from subframe grid: abs %0d vs %0d",
seeded_epoch_abs, subframe_epoch_abs);
$display(" [3] both paths land on global grid at absolute bit %0d",
seeded_epoch_abs);
endtask
// --------------------------------------------------------------------
// 4. An unreduced seed is refused, not aliased onto a wrong grid.
// --------------------------------------------------------------------
task automatic test_unreduced_seed_rejected();
reset_dut();
enable = 1'b1;
trck_busy = 1'b1;
arm_seed(COUNTER_MAX); // == counter_max_val, out of range
if(nav_solve_provisional)
$fatal(1, "out-of-range seed was accepted");
if(consumed_count == 0)
$fatal(1, "out-of-range seed left the arm bit set");
epoch_count = 0;
nav_bits(4 * COUNTER_MAX);
repeat(8) @(posedge clk);
if(epoch_count != 0)
$fatal(1, "rejected seed still produced epochs: %0d", epoch_count);
$display(" [4] unreduced seed rejected and arm bit cleared");
endtask
// --------------------------------------------------------------------
// 5. A seed offered while the channel is not busy is refused.
// --------------------------------------------------------------------
task automatic test_seed_requires_busy_and_enable();
reset_dut();
enable = 1'b1;
trck_busy = 1'b0;
arm_seed(2);
if(nav_solve_provisional)
$fatal(1, "seed accepted while tracking was not busy");
reset_dut();
enable = 1'b0;
trck_busy = 1'b1;
arm_seed(2);
if(nav_solve_provisional)
$fatal(1, "seed accepted while the timer was disabled");
$display(" [5] seed requires busy and enable");
endtask
// --------------------------------------------------------------------
// 6. A seed arriving while epochs already run is ignored.
// --------------------------------------------------------------------
task automatic test_seed_ignored_while_counting();
int unsigned z;
int unsigned phase;
int unsigned epoch_bit_before;
int unsigned epoch_bit_after;
z = 5;
phase = (z * 300) % COUNTER_MAX;
reset_dut();
enable = 1'b1;
trck_busy = 1'b1;
nav_bit_with_subframe(z);
last_epoch_bit_valid = 1'b0;
nav_bits(2 * COUNTER_MAX);
repeat(8) @(posedge clk);
epoch_bit_before = last_epoch_bit;
/* Offer a phase deliberately different from the running one. */
arm_seed((phase + 4) % COUNTER_MAX);
if(nav_solve_provisional)
$fatal(1, "running phase was overwritten by a late seed");
last_epoch_bit_valid = 1'b0;
nav_bits(COUNTER_MAX);
repeat(8) @(posedge clk);
epoch_bit_after = last_epoch_bit;
if(((epoch_bit_after - epoch_bit_before) % COUNTER_MAX) != 0)
$fatal(1, "epoch grid shifted after a late seed: %0d -> %0d",
epoch_bit_before, epoch_bit_after);
$display(" [6] late seed ignored, running grid preserved");
endtask
// --------------------------------------------------------------------
// 7. A wrong seed self-corrects on the first decoded subframe.
// --------------------------------------------------------------------
task automatic test_subframe_overrides_wrong_seed();
int unsigned z;
int unsigned phase;
int unsigned wrong_phase;
int unsigned expected_bit_mod;
reset_dut();
enable = 1'b1;
trck_busy = 1'b1;
z = 9;
phase = (z * 300) % COUNTER_MAX;
wrong_phase = (phase + 3) % COUNTER_MAX;
arm_seed(wrong_phase);
if(!nav_solve_provisional)
$fatal(1, "seed was not taken");
nav_bits(COUNTER_MAX + 2);
/* Now the real subframe arrives. It must re-align and clear provisional. */
nav_bit_with_subframe(z);
if(nav_solve_provisional)
$fatal(1, "provisional flag survived a decoded subframe");
/* After realignment the counter holds (z*300) mod N at bit_index, so the
* next epoch is (COUNTER_MAX-1-phase) bits later. */
epoch_count = 0;
nav_bits(COUNTER_MAX - 1 - phase);
if(epoch_count != 0)
$fatal(1, "epoch fired early after realignment");
nav_bit();
repeat(8) @(posedge clk);
if(epoch_count != 1)
$fatal(1, "realigned epoch missing");
$display(" [7] decoded subframe overrides a wrong seed");
endtask
// --------------------------------------------------------------------
// 8. Stale-seed safety: an armed seed cannot survive a channel drop.
// --------------------------------------------------------------------
task automatic test_seed_cannot_survive_a_drop();
int unsigned z;
z = 3;
reset_dut();
enable = 1'b1;
trck_busy = 1'b1;
nav_bit_with_subframe(z);
nav_bits(3);
/* Arm while COUNT is running; hardware must answer and discard it. */
arm_seed(1);
if(nav_solve_provisional)
$fatal(1, "seed applied while counting");
/* Channel drops, returning the timer to IDLE. */
trck_busy = 1'b0;
repeat(8) @(posedge clk);
if(nav_solve_provisional)
$fatal(1, "provisional flag survived a channel drop");
trck_busy = 1'b1;
repeat(8) @(posedge clk);
/* Nothing was left armed, so no epochs may appear without a new seed. */
epoch_count = 0;
nav_bits(4 * COUNTER_MAX);
repeat(8) @(posedge clk);
if(epoch_count != 0)
$fatal(1, "stale seed restarted epochs after a drop: %0d", epoch_count);
$display(" [8] armed seed cannot survive a channel drop");
endtask
// --------------------------------------------------------------------
// 9. A subframe in the same cycle as a seed wins.
// --------------------------------------------------------------------
task automatic test_subframe_wins_tie();
int unsigned z;
int unsigned phase;
z = 13;
phase = (z * 300) % COUNTER_MAX;
reset_dut();
enable = 1'b1;
trck_busy = 1'b1;
phase_seed_value = 32'((phase + 5) % COUNTER_MAX);
phase_seed_arm = 1'b1;
nav_bit_with_subframe(z);
phase_seed_arm = 1'b0;
repeat(4) @(posedge clk);
if(nav_solve_provisional)
$fatal(1, "seed won a tie against a decoded subframe");
epoch_count = 0;
nav_bits(COUNTER_MAX - 1 - phase);
if(epoch_count != 0)
$fatal(1, "tie resolution produced the seeded grid");
nav_bit();
repeat(8) @(posedge clk);
if(epoch_count != 1)
$fatal(1, "tie resolution lost the subframe grid");
$display(" [9] decoded subframe wins a same-cycle tie");
endtask
initial begin
test_subframe_path_unchanged();
test_seed_starts_epochs();
test_seed_matches_subframe_grid();
test_unreduced_seed_rejected();
test_seed_requires_busy_and_enable();
test_seed_ignored_while_counting();
test_subframe_overrides_wrong_seed();
test_seed_cannot_survive_a_drop();
test_subframe_wins_tie();
$display("nav_solve_timer_seed_tb PASS");
$finish;
end
endmodule
hardware/external/gps/rtl/readme.md
Vendored
+44
View File
@@ -69,11 +69,55 @@ payload'dan once ve sonra okuyup esitligini kontrol eder.
| `0x68` | `MEASUREMENT_SUM_Q` | Signed coherent prompt-Q toplami |
| `0x6c` | `MEASUREMENT_WBP_LO` | Wideband power alt 32 bit |
| `0x70` | `MEASUREMENT_WBP_HI` | Wideband power ust bitleri |
| `0x74` | `NAV_SOLVE_PHASE_SEED` | Warm/hot reacquisition icin navigation-bit fazi |
`CONTROL_REG` bit 13 measurement interrupt enable, bit 14 measurement flag
enable'dir. `STATUS_REG` bit 7 measurement flag'dir. Genel interrupt clear
islemi bu flag'i de temizler.
### NAV solve phase seed (warm/hot reacquisition)
`nav_solve_timer` normalde IDLE'dan yalniz `subframe_ready` ile cikar ve fazini
HOW Z-count'tan `(Z * 300) mod N` olarak hesaplar. Kanal yeniden kilitlendiginde
`trck_busy` dustugu icin timer IDLE'a doner; yani bir uydu geri geldiginde
ephemeris RAM cache'te hazir olsa bile yeni bir subframe cozulene kadar (en fazla
6 s) hic NAV solve pulse'i uretilmez.
Bunun icin `NAV_SOLVE_PHASE_SEED` (`0x74`) ve `CONTROL_REG` bit 15
(`TRCK_NAV_SOLVE_SEED_ARM`) eklendi. Software once fazi yazar, sonra arm bitini
set eder.
**Seed anlami:** seed, timer istegi tukettikten sonra gelen *bir sonraki*
navigation bitinde gecerli olacak sayac degeridir. Donanim sayac `N-1` iken epoch
uretir, dolayisiyla:
```text
seed = (bir sonraki navigation bitinin mutlak indeksi) mod N
```
Bu, subframe yolunun urettigi izgaranin aynisidir: `Z * 300` zaten subframe
sinirindan sonraki ilk bitin mutlak indeksidir. Iki yol da `(b + 1) mod N == 0`
olan mutlak bit indekslerine dusen tek bir global izgara uretir.
Donanim garantileri (her biri
`gps_timer/tb/nav_solve_timer_seed_tb.sv` icinde bir test ile karsilanir):
- Arm istegi **her state'te** bir cevrimde cevaplanir ve `phase_seed_consumed`
ile temizlenir; boylece bir seed sonraki bir reacquisition'a tasinmaz.
- Seed yalniz IDLE'da uygulanir; ayni cevrimde gelen `subframe_ready` kazanir.
- `phase_seed_value >= counter_max_val` ise seed reddedilir (yanlis izgaraya
alias yapilmaz), ama arm biti yine temizlenir; software beklemede kalmaz.
- Seed ile baslayan kanal `STATUS_REG` bit 8
(`TRCK_NAV_SOLVE_PROVISIONAL`) ile isaretlenir. Ilk cozulen subframe
otoriter HOW hizalamasini tekrar calistirir ve bu biti temizler: yanlis bir
seed en fazla bir subframe icinde kendini duzeltir.
- `trck_busy` dustugunde timer IDLE'a doner ve provisional temizlenir.
Bu register'i olmayan eski bir bitstream'e karsi yazmak APB `default` case'ine
duser ve `PSLVERR` uretir; `TLToAPB` bunu `denied` olarak dondurdugu icin Rocket
store access fault alabilir. Bu yuzden firmware tarafinda
`GPS_WARM_SEED_ENABLE=0` build secenegi vardir.
### Neden Doppler IIR RTL'de degil?
IIR RTL'de de uygulanabilir; her kanal icin onceki Doppler state'i, signed
hardware/external/gps/rtl/trck_src/tb/Makefile
Vendored
+7 -1
View File
@@ -14,13 +14,19 @@ test:
-f filelist.f \
$(CURDIR)/lock_detector_energy_tb.sv \
$(CURDIR)/tracking_measurement_tb.sv \
$(CURDIR)/tracking_wrapper_measurement_tb.sv
$(CURDIR)/tracking_wrapper_measurement_tb.sv \
$(CURDIR)/tracking_wrapper_seed_tb.sv \
$(RTL_DIR)/gps_timer/tb/nav_solve_timer_seed_tb.sv
cd $(QUESTA_RUN_DIR) && $(QUESTA_VSIM) -c tracking_measurement_tb \
-do 'run -all; quit -f'
cd $(QUESTA_RUN_DIR) && $(QUESTA_VSIM) -c lock_detector_energy_tb \
-do 'run -all; quit -f'
cd $(QUESTA_RUN_DIR) && $(QUESTA_VSIM) -c tracking_wrapper_measurement_tb \
-do 'run -all; quit -f'
cd $(QUESTA_RUN_DIR) && $(QUESTA_VSIM) -c nav_solve_timer_seed_tb \
-do 'run -all; quit -f'
cd $(QUESTA_RUN_DIR) && $(QUESTA_VSIM) -c tracking_wrapper_seed_tb \
-do 'run -all; quit -f'
clean:
rm -rf $(QUESTA_RUN_DIR)
hardware/external/gps/rtl/trck_src/tb/tracking_wrapper_seed_tb.sv
Vendored
+255
View File
@@ -0,0 +1,255 @@
`timescale 1ns/1ps
import definitions::*;
import trck_pkg::*;
/*
* APB-level contract for the warm/hot reacquisition phase seed:
* - NAV_SOLVE_PHASE_SEED is a normal read/write register,
* - the arm bit in CONTROL is answered by hardware and self-clears,
* - STATUS reports whether epochs are running on an unconfirmed seed,
* - a seeded channel raises real nav-solve interrupts and PPS captures
* with no decoded subframe.
*/
module tracking_wrapper_seed_tb;
localparam int unsigned COUNTER_MAX = 10;
logic PCLK = 1'b0;
logic PRESETN = 1'b0;
logic [11:0] PADDR = '0;
logic PSEL = 1'b0;
logic PENABLE = 1'b0;
logic PWRITE = 1'b0;
logic [31:0] PWDATA = '0;
logic PREADY;
logic [31:0] PRDATA;
logic PSLVERR;
logic trck_resetn = 1'b0;
logic afe_resetn = 1'b0;
logic trck_clk = 1'b0;
logic afe_clk = 1'b0;
logic [3:0] afe_data = '0;
logic sample_align = 1'b0;
logic [31:0] sample_counter = '0;
logic gps_frame_ready;
logic pps_capture;
integer interrupt_count = 0;
integer capture_count = 0;
always #5 PCLK = ~PCLK;
always #4 trck_clk = ~trck_clk;
always #8 afe_clk = ~afe_clk;
always @(posedge PCLK) begin
if(gps_frame_ready) interrupt_count++;
if(pps_capture) capture_count++;
end
tracking_wrapper #(
.LOW_ADDRESS(0),
.HIGH_ADDRESS(4095),
.ADDR_BUS_WIDTH(12)
) dut (
.PCLK, .PRESETN, .PADDR, .PSEL, .PENABLE, .PWRITE, .PWDATA,
.PREADY, .PRDATA, .PSLVERR,
.trck_resetn, .afe_resetn, .trck_clk, .afe_clk, .afe_data,
.sample_align, .sample_counter, .gps_frame_ready, .pps_capture
);
task automatic pclk_cycles(input int unsigned count);
repeat(count) @(posedge PCLK);
endtask
task automatic apb_write(input logic [11:0] address,
input logic [31:0] data);
@(negedge PCLK);
PADDR = address;
PWDATA = data;
PWRITE = 1'b1;
PSEL = 1'b1;
PENABLE = 1'b0;
@(posedge PCLK);
#1;
@(negedge PCLK);
PENABLE = 1'b1;
@(posedge PCLK);
#1;
@(negedge PCLK);
PSEL = 1'b0;
PENABLE = 1'b0;
PWRITE = 1'b0;
pclk_cycles(2);
endtask
task automatic apb_read(input logic [11:0] address,
output logic [31:0] data);
@(negedge PCLK);
PADDR = address;
PWRITE = 1'b0;
PSEL = 1'b1;
PENABLE = 1'b0;
@(posedge PCLK);
#1;
data = PRDATA;
@(negedge PCLK);
PSEL = 1'b0;
pclk_cycles(2);
endtask
task automatic expect_read(input logic [11:0] address,
input logic [31:0] expected);
logic [31:0] value;
apb_read(address, value);
if(value !== expected)
$fatal(1, "APB read 0x%03x mismatch: got=0x%08x expected=0x%08x",
address, value, expected);
endtask
/* One navigation bit as the tracking core presents it: a bit_valid pulse in
* the tracking clock domain, wide enough to cross the wrapper's two-flop
* synchroniser. */
task automatic nav_bit();
@(negedge trck_clk);
force dut.bit_valid = 1'b1;
repeat(4) @(posedge PCLK);
@(negedge trck_clk);
force dut.bit_valid = 1'b0;
repeat(8) @(posedge PCLK);
endtask
task automatic nav_bits(input int unsigned count);
for(int unsigned i = 0; i < count; i++) nav_bit();
endtask
logic [31:0] control_base;
logic [31:0] status_value;
int unsigned phase;
initial begin
force dut.lock = 1'b1;
force dut.busy = 1'b1;
force dut.bit_valid = 1'b0;
force dut.measurement_valid = 1'b0;
pclk_cycles(5);
PRESETN = 1'b1;
trck_resetn = 1'b1;
afe_resetn = 1'b1;
pclk_cycles(5);
control_base = (1 << TRCK_INTRPT_ENABLE_POS) |
(1 << TRCK_NAV_SOLVE_ENABLE_POS) |
(1 << TRCK_NAV_SOLVE_INTR_EN_POS) |
(1 << TRCK_NAV_SOLVE_FLAG_EN_POS);
apb_write(NAV_SOLVE_TIMER_REG_OFFSET[11:0], 32'(COUNTER_MAX));
apb_write(CONTROL_REG_OFFSET[11:0], control_base);
// ----------------------------------------------------------------
// The seed register is plain read/write storage.
// ----------------------------------------------------------------
apb_write(NAV_SOLVE_PHASE_SEED_REG_OFFSET[11:0], 32'd7);
expect_read(NAV_SOLVE_PHASE_SEED_REG_OFFSET[11:0], 32'd7);
// ----------------------------------------------------------------
// Nothing runs before the seed is armed: no subframe has been decoded.
// ----------------------------------------------------------------
interrupt_count = 0;
capture_count = 0;
nav_bits(3 * COUNTER_MAX);
if(interrupt_count !== 0)
$fatal(1, "nav solve ran without a subframe or a seed: %0d",
interrupt_count);
// ----------------------------------------------------------------
// Arm the seed; hardware must answer it and clear the arm bit itself.
// ----------------------------------------------------------------
phase = 3;
apb_write(NAV_SOLVE_PHASE_SEED_REG_OFFSET[11:0], 32'(phase));
apb_write(CONTROL_REG_OFFSET[11:0],
control_base | (1 << TRCK_NAV_SOLVE_SEED_ARM_POS));
pclk_cycles(6);
apb_read(CONTROL_REG_OFFSET[11:0], status_value);
if(status_value[TRCK_NAV_SOLVE_SEED_ARM_POS] !== 1'b0)
$fatal(1, "arm bit did not self-clear: control=0x%08x", status_value);
apb_read(STATUS_REG_OFFSET[11:0], status_value);
if(status_value[TRCK_NAV_SOLVE_PROVISIONAL_POS] !== 1'b1)
$fatal(1, "provisional status not reported: status=0x%08x", status_value);
// ----------------------------------------------------------------
// The seeded channel now produces nav-solve interrupts and PPS captures.
// ----------------------------------------------------------------
interrupt_count = 0;
capture_count = 0;
nav_bits(COUNTER_MAX - 1 - phase);
if(interrupt_count !== 0)
$fatal(1, "seeded epoch fired early: %0d", interrupt_count);
nav_bit();
pclk_cycles(8);
if(interrupt_count !== 1)
$fatal(1, "seeded nav-solve interrupt missing: %0d", interrupt_count);
if(capture_count !== 1)
$fatal(1, "seeded epoch did not request a PPS capture: %0d",
capture_count);
apb_read(STATUS_REG_OFFSET[11:0], status_value);
if(status_value[TRCK_NAV_SOLVE_FLAG_POS] !== 1'b1)
$fatal(1, "nav solve flag not set: status=0x%08x", status_value);
apb_write(CONTROL_REG_OFFSET[11:0],
control_base | (1 << TRCK_INTRPT_CLEAR_POS));
// ----------------------------------------------------------------
// Epochs keep their period.
// ----------------------------------------------------------------
interrupt_count = 0;
nav_bits(COUNTER_MAX);
pclk_cycles(8);
if(interrupt_count !== 1)
$fatal(1, "seeded epoch is not periodic: %0d", interrupt_count);
// ----------------------------------------------------------------
// A channel drop returns the timer to idle and clears provisional; the
// stale seed value in the register must not restart anything.
// ----------------------------------------------------------------
force dut.busy = 1'b0;
pclk_cycles(12);
apb_read(STATUS_REG_OFFSET[11:0], status_value);
if(status_value[TRCK_NAV_SOLVE_PROVISIONAL_POS] !== 1'b0)
$fatal(1, "provisional survived a drop: status=0x%08x", status_value);
force dut.busy = 1'b1;
pclk_cycles(12);
interrupt_count = 0;
nav_bits(3 * COUNTER_MAX);
pclk_cycles(8);
if(interrupt_count !== 0)
$fatal(1, "stale seed restarted epochs after a drop: %0d",
interrupt_count);
// ----------------------------------------------------------------
// An out-of-range seed is refused but still clears the arm bit, so
// firmware can never wedge waiting for an answer.
// ----------------------------------------------------------------
apb_write(NAV_SOLVE_PHASE_SEED_REG_OFFSET[11:0], 32'(COUNTER_MAX + 5));
apb_write(CONTROL_REG_OFFSET[11:0],
control_base | (1 << TRCK_NAV_SOLVE_SEED_ARM_POS));
pclk_cycles(6);
apb_read(CONTROL_REG_OFFSET[11:0], status_value);
if(status_value[TRCK_NAV_SOLVE_SEED_ARM_POS] !== 1'b0)
$fatal(1, "rejected seed left the arm bit set: control=0x%08x",
status_value);
apb_read(STATUS_REG_OFFSET[11:0], status_value);
if(status_value[TRCK_NAV_SOLVE_PROVISIONAL_POS] !== 1'b0)
$fatal(1, "out-of-range seed was accepted: status=0x%08x", status_value);
interrupt_count = 0;
nav_bits(3 * COUNTER_MAX);
pclk_cycles(8);
if(interrupt_count !== 0)
$fatal(1, "rejected seed produced epochs: %0d", interrupt_count);
$display("tracking_wrapper_seed_tb PASS");
$finish;
end
endmodule
soc/software/gps_system/Makefile
+24
View File
@@ -68,6 +68,11 @@ APP_SRCS := \
src/gps/gps_interrupt_handlers.c \
src/gps/gps_acq_manager.c \
src/gps/gps_ephemeris_cache.c \
src/gps/gps_ephemeris_validity.c \
src/gps/gps_ephemeris_store.c \
src/gps/gps_lnav_parser.c \
src/gps/gps_almanac.c \
src/gps/gps_nav_context_store.c \
src/gps/gps_time_anchor.c \
src/gps/gps_nav_epoch.c \
src/gps/gps_pps.c \
@@ -81,6 +86,21 @@ APP_SRCS := \
BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c
GPS_NAV_SOLVE_TIMER_TICKS ?= 10
# Warm/hot reacquisition epoch-phase seeding. This needs the tracking IP's
# NAV_SOLVE_PHASE_SEED register, added alongside this firmware. Building the
# feature in and running against an older bitstream would make the write decode
# to PSLVERR, which the APB-to-TileLink bridge reports as `denied` and Rocket
# can take as a store access fault. Set to 0 to build firmware for a bitstream
# that predates the register.
GPS_WARM_SEED_ENABLE ?= 1
# Persistent ephemeris cache in the top two 64 KiB flash sectors. Set to 0 to
# build firmware that never writes to flash.
GPS_EPHEMERIS_FLASH_ENABLE ?= 1
# Test hooks. MAX_SLOTS caps slots per sector so flash compaction is reachable
# on hardware in seconds instead of after a full sector; FORCE_FORMAT erases
# both sectors at boot. Both must be 0 in a production image.
GPS_EPHEMERIS_STORE_MAX_SLOTS ?= 0
GPS_EPHEMERIS_FLASH_FORCE_FORMAT ?= 0
GPS_PPS_STATUS_LOG_INTERVAL ?= 10
GPS_PPS_DOPPLER_PREDICTOR_ENABLE ?= 1
GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES ?= 0
@@ -96,6 +116,10 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \
-DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \
-DIMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=$(IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES) \
-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS) \
-DGPS_WARM_SEED_ENABLE=$(GPS_WARM_SEED_ENABLE) \
-DGPS_EPHEMERIS_FLASH_ENABLE=$(GPS_EPHEMERIS_FLASH_ENABLE) \
-DGPS_EPHEMERIS_STORE_MAX_SLOTS=$(GPS_EPHEMERIS_STORE_MAX_SLOTS)u \
-DGPS_EPHEMERIS_FLASH_FORCE_FORMAT=$(GPS_EPHEMERIS_FLASH_FORCE_FORMAT) \
-DGPS_PPS_STATUS_LOG_INTERVAL=$(GPS_PPS_STATUS_LOG_INTERVAL) \
-DGPS_PPS_DOPPLER_PREDICTOR_ENABLE=$(GPS_PPS_DOPPLER_PREDICTOR_ENABLE) \
-DGPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=$(GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES)
soc/software/gps_system/README.md
+192
View File
@@ -4,6 +4,11 @@ Bu notlar GPS bare-metal yaziliminin VCU118/Genesys2 Rocket SoC portunu,
tracking Doppler/C/N0 yolunu, ESKF entegrasyonunu ve interrupt debug surecini
belgeler.
Alici zaman tabani (ABS_SAMPLE / PPS sayaci), CDC dogrulamasi, navigation epoch
uretimi, 68 ms STARTOFFSET'in ne oldugu, ephemeris gecerlilik kurallari ve
warm/hot reacquisition gecikmesinin nerede sinirlandigi ayri bir belgede
toplanmistir: [docs/RECEIVER_TIMING_AND_REACQUISITION.md](docs/RECEIVER_TIMING_AND_REACQUISITION.md).
## Uygulama akisi ve dosya sinirlari
Firmware giris noktasi [gps_system.c](gps_system.c)'dir. `main()` surucu veya
@@ -171,6 +176,23 @@ make FPGA_BOARD=genesys2 APP=gps_system \
GPS_ESKF_ENABLE=0 fpga-mcs
```
Warm/hot reacquisition ve kalici ephemeris build secenekleri:
```sh
# Varsayilan: ikisi de acik. NAV_SOLVE_PHASE_SEED register'ini iceren
# bitstream gerekir.
make FPGA_BOARD=genesys2 APP=gps_system fpga-mcs
# Register'i olmayan eski bir bitstream icin: seed yolu derlemeden cikar.
# Aksi halde APB PSLVERR -> denied -> store access fault riski vardir.
make FPGA_BOARD=genesys2 APP=gps_system GPS_WARM_SEED_ENABLE=0 fpga-mcs
# Flash'a hic yazmayan firmware.
make FPGA_BOARD=genesys2 APP=gps_system GPS_EPHEMERIS_FLASH_ENABLE=0 fpga-mcs
```
Bu iki bayrak `targets/fpga/genesys2/Makefile` uzerinden de forward edilir.
Her iki profil bootloader ve uygulamayi varsayilan `460800` baud ile derler.
Iki komut ayni MCS/BIN yoluna yazdigi icin iki imaj birlikte saklanacaksa
build'ler arasinda profil adiyla kopyalanmalidir. Bu secenekler software
@@ -364,6 +386,23 @@ Generated DTS/RTL'e gore GPS IP adresleri:
Bu degerler `soc/software/bsp/include/bsp/soc.h` icinde BSP define olarak tutuluyor. GPS yazilimi `header/gps/gps_ip_defines.h` icinde bu BSP define'larini kullaniyor.
Tracking IP register haritasina warm/hot reacquisition icin eklenen alanlar:
| Offset | Alan | Yon | Aciklama |
| ---: | --- | --- | --- |
| `0x74` | `NAV_SOLVE_PHASE_SEED` | R/W | Navigation-bit fazi; `< GPS_NAV_SOLVE_TIMER_TICKS` olmali |
| `CONTROL_REG` bit 15 | `GPS_TRCK_NAV_SOLVE_SEED_ARM_POS` | R/W | Seed'i arm eder; donanim cevapladiginda kendisi temizler |
| `STATUS_REG` bit 8 | `GPS_TRCK_NAV_SOLVE_PROVISIONAL_POS` | R | Epoch'lar henuz subframe ile dogrulanmamis bir seed uzerinde |
Protokol: once `NAV_SOLVE_PHASE_SEED` yazilir, sonra control bit 15 set edilir.
Donanim istegi bir cevrimde cevaplar; arm biti 0'a donduginde status bit 8
seed'in kabul edilip edilmedigini soyler. Ayrintili davranis ve seed formulu
`hardware/external/gps/rtl/readme.md` icindeki "NAV solve phase seed" bolumunde.
Flash'ta kalici ephemeris cache'i 32 MiB S25FL256S'in son iki 64 KiB
sektorundedir (`GPS_EPHEMERIS_FLASH_BASE = 0x01FE0000`); bitstream `0x00000000`
ve uygulama `0x01800000` bolgeleriyle cakismaz.
PLIC base:
```text
@@ -904,6 +943,47 @@ zamani cikarilir ve `STARTOFFSET` eklenir. Dolayisiyla
datumdur. Doppler `solution.clock_drift_mps` ile bunun arasinda turev kisiti
kurulmaz.
#### 68 ms STARTOFFSET neden duruyor (bilincli karar)
Serbest calisan sayaclar (32-bit ABS_SAMPLE, 64-bit PPS) `STARTOFFSET`'i
**degistirmez ve degistirmesi de amaclanmamistir**. Ikisi farkli problemi cozer:
- `STARTOFFSET` mutlak **alis** zamaniyla ilgilidir. `gps_ip.c` icindeki
pseudorange kurulumunda alis zamani hic gecmez:
```text
preciseTxTime_i = t_tx_i - t_tx_reference + 68.802 ms
pseudorange_i = preciseTxTime_i * c
```
Yani bu, referans uyduya gore bir **transmit-time farki** artı makul bir
transit suresi sabitidir. Sabit butun kanallarda ayni oldugu icin WLS onu
tamamen `b` (clock bias) durumuna yutar.
- Serbest calisan sayaclar ise **fark** verir, mutlak datum degil. HOW ->
transmit time ankraji, epoch kimligi, warm-start seed'i ve PRN baglami
bunlarla calisir; hicbiri mutlak bir alis epoch'u uretmez.
Ayrica `STARTOFFSET` her cozumde yeniden hesaplanan bir sabittir, tasinan bir
state degildir; bu yuzden reacquisition sirasinda bayatlayamaz ve warm-start
yolunu etkilemez.
Kart dogrulamasi (2026-08-08): beklenen datum `c*STARTOFFSET - reference_range`
= `20.626 km - ~20.372 km` yaklasik `254 km`. Karttan okunan
`filtered_bias` yaklasik `253.686 km`. Model canli veriyle ortusuyor.
**Degistirilmedi, cunku gerekli degil.** Ileride PPS disiplinli mutlak alis
zamanina gecilmek istenirse gereken is (bu surumde kapsam disidir):
1. `pseudorange_i = (t_rx - t_tx_i) * c` seklinde PPS disiplinli mutlak `t_rx`
kullanmak.
2. Cold start'ta yumurta-tavuk problemi: PPS disiplini bir fix gerektirir, fix
de bu durumda PPS'i gerektirir; kilit oncesi bir bootstrap yolu gerekir.
3. `b` yaklasik 254 km'den gercek (kucuk) receiver bias'ina duser; bu nedenle
`gps_pps_time_filter` (header'inda bu datum acikca yazilidir), residual
kapilari ve ESKF/GPS adapter kovaryanslari yeniden ayarlanmalidir.
Bu, calisan bir fix uzerinde gercek regresyon riski tasiyan ayri bir istir.
Fit'ler matematiksel olarak ayridir ve `solution_valid`/`velocity_valid`
bagimsizdir. Bununla birlikte velocity WLS LOS birim vektorlerini hesaplamak
icin position WLS'nin guncel receiver ECEF konumunu kullanir; uydu ECEF
@@ -1018,6 +1098,14 @@ tanimlanan Makefile varsayilanlarini ezer.
### Register Map Dogrulama
Kart uzerinde dogrulanan (2026-08-08, Genesys2):
- `NAV_SOLVE_PHASE_SEED` (`0x10021074`) yazilip geri okunuyor: `0x5A` yazildi,
`0x5A` okundu, `0` ile geri alindi. Register bitstream'de mevcut.
- `STATUS_REG` bit 8 (`NAV_SOLVE_PROVISIONAL`) subframe ile hizalanmis bir
kanalda `0` okunuyor; `trck0 STATUS = 0xdb`.
- `CONTROL_REG` bit 15 arm edildikten sonra donanim tarafindan temizleniyor.
XLSX register dokumanlarina gore dogrulananlar:
- ACQ offset'leri firmware ile uyumlu:
@@ -1327,6 +1415,110 @@ RTL'de `acq_top.sv` akisi:
Bu timer/scheduler yavasligi degildir. Firmware start ediyor; tamamlanma icin RF/AFE clock ve sample-align path'i board uzerinde dogrulanmalidir.
### Warm/Hot Reacquisition ve Kalici Ephemeris
Zaman olcekleri karistirilmamali:
| Periyot | Nedir | Neyi belirler |
| --- | --- | --- |
| 20 ms | bir navigation biti | donanim sayma birimi, bit senkronu |
| **200 ms** | NAV solve epoch (`GPS_NAV_SOLVE_TIMER_TICKS = 10`) | bir uydunun WLS cozumune **katilabilecegi** an |
| 6 s | subframe (300 bit) | ephemeris/HOW cozumu |
| 30 s | frame (5 subframe) | tam ephemeris seti |
Onceden geri donen bir uydu **6 s** seviyesinde kilitliydi. Sebep ephemeris
degildi (RAM cache zaten SF1-3 beklemesini kaldiriyordu); sebep
`nav_solve_timer`'in `subframe_ready` olmadan IDLE'dan cikmamasiydi. Yeni seed
ile kanal **200 ms** izgarasinda hemen baslar.
Akis:
1. Takip sirasinda her kabul edilen epoch, PRN bazli bir baglam kaydeder
(transmit time + PPS counter + sample latch, hepsi tek donanim pulse'indan).
Anahtar PRN'dir, kanal degil; kanal serbest kalinca kaybolmaz.
2. Drop'ta kanal state'i silinir, PRN baglami ve ephemeris cache'i bilerek kalir.
3. Reacquire'da ephemeris RAM cache'ten (yoksa flash'tan) gelir. Firmware fazi
PPS domaininde hesaplar ve `NAV_SOLVE_PHASE_SEED` ile arm eder.
4. Donanim dogru global izgarada saymaya baslar, `PROVISIONAL` isaretlenir.
5. Ilk seed'li epoch 200 ms izgarasina snap edilir; bu hem tam GPS zamanini geri
kazandirir hem de baglamin hala gecerli oldugunu kanitlar. Kanal ancak bundan
sonra beklenen kumeye girer.
6. 6 s'lik subframe arka planda gelmeye devam eder, otoriter HOW hizalamasini
tekrar calistirir ve `PROVISIONAL`'i temizler. Yani 6 s artik bir kapi degil,
dogrulama/tazeleme yoludur.
Yeni alt sinir: acquisition + tracking lock + bir 200 ms epoch. Sayaclarin
gecisi kartta dogrulandi; duvar saati gecikmesi henuz olculmedi.
Tani sayaclari `gps_runtime_diagnostics` icindedir: `warm_seed_armed_total`,
`warm_seed_confirmed_total`, `warm_seed_rejected_total`,
`ephemeris_cache_restore_total`, `ephemeris_cache_miss_total`,
`flash_ephemeris_loaded_total`, `flash_ephemeris_write_total`,
`flash_ephemeris_error_total`.
Flash store test kancalari (production imajinda ikisi de `0` olmali):
```sh
# Her iki sektoru boot'ta sil; format yolunu gozlemlemek icin.
make FPGA_BOARD=genesys2 APP=gps_system \
GPS_EPHEMERIS_FLASH_FORCE_FORMAT=1 fpga-mcs
# Sektor basina kullanilabilir slot sayisini kis; compaction'i 256 kayit
# beklemeden, gercek 64 KiB erase'lerle saniyeler icinde tetikler.
make FPGA_BOARD=genesys2 APP=gps_system \
GPS_EPHEMERIS_STORE_MAX_SLOTS=4 fpga-mcs
```
Yeni bir build bayragi eklerken **uc yere** birden eklenmelidir, aksi halde
sessizce dusurulur ve kartta hicbir etkisi olmaz:
`gps_ip_defines.h` icindeki `#ifndef` varsayilani,
`soc/software/gps_system/Makefile`, ve `targets/fpga/genesys2/Makefile`.
#### Total outage sonrasi yavas toparlanma: iki yazilim hatasi
RF kablosu cekildiginde konumun yeniden basmasi yaklasik bir dakika suruyordu ve
yeni bir uydu cozume katildiginda cikti saniyelerce duruyordu. Iki ayri yazilim
hatasi bulundu; ikisi de RTL degisikligi gerektirmedi.
**Yanlis teshise yol acan sayac.** `nav_epoch_incomplete_total` uc ayri yerden
artiriliyordu ve bunlardan biri bambaska bir sey demekti: epoch dogru toplandi
ama `GPS_NAV_MIN_WLS_SATELLITES`'ten az uydu tasidi. Kartta 633 sayimin 563'u
bu durumdu, yalnizca 70'i gercek assembly hatasiydi. Sayac artik
`nav_epoch_assembly_failed_total` ve `nav_epoch_below_minimum_total` olarak
ayrildi; eskisi yalniz sureklilik icin duruyor, teshiste kullanilmamali.
**Hata 1 - acquisition retry onceligi tersti.** Saniyeler once takip edilen
(HOT) bir uydu ilk denemesinden itibaren 30 s backoff'a giriyordu; hic
gorulmemis (COLD) bir PRN ise her 0.5 s'de tekrar deneniyordu. Yani en iyi aday
en kotusunden altmis kat agir cezalandiriliyordu. Artik tazelik
`GPS_ACQ_HOT_FAST_ATTEMPTS` (6) hizli deneme hakki veriyor ve bu hak yalniz
**basarisizliklarla** tukeniyor.
**Hata 2 - nav-solve etkinlestirmede dairesel bagimlilik.** `gps_ip.c` icinde
nav-solve interrupt'i yalniz `ready_count >= GPS_NAV_MIN_WLS_SATELLITES` iken
aciliyordu. Ama bir kanal nav-solve acilmadan epoch'a katkida bulunamaz; yani
dort uydunun altina dusuldugunde alici bir daha yukari cikamiyordu. Warm-seed
yolu nav-solve'u bagimsiz actigi icin hata bazen maskeleniyordu; teshisi
zorlastiran da buydu. Etkinlestirme artik uydu sayisindan bagimsiz, kanal bazinda
yapiliyor.
Kart dogrulamasi: dort kanal takipteyken ve fix yokken dordunde de
`navsolve_irq_en=1` okunuyor; eski kodda bu durum erisilemezdi.
**Henuz dogrulanmadi:** iki duzeltmenin de mekanizmasi dogrulandi, fakat
toparlanma **suresi** dogrulanmadi. Test sirasinda gokyuzu zayifladi (once 7-8,
sonra 4, en son 2 uydu) ve dort uydunun altinda hicbir kodla fix uretilemez.
7-8 uyduyla tekrar drop testi gerekiyor.
**Onemli:** bu yollarin GPS_LOG mesajlari `DEBUG_INFO` yani seviye 3'tedir ve
seviye 3 bu kartta 460800 baud UART'i doyurup foreground'u ac birakir
(`tracked=0 stale=5` gorulur). Kartta tani icin seviye 1'de kalip sayaclari
JTAG uzerinden okuyun; printf ile kovalamayin.
Detayli mimari, CDC analizi, 68 ms STARTOFFSET aciklamasi, ephemeris gecerlilik
kurallari, flash format tasarimi ve kart dogrulama sonuclari:
[docs/RECEIVER_TIMING_AND_REACQUISITION.md](docs/RECEIVER_TIMING_AND_REACQUISITION.md).
### Kalan Riskler ve Board-Level Test Ihtiyaci
- Mevcut donanim/BSP tek ACQ engine expose ediyor. Birden fazla ACQ engine icin hardware/BSP register map'i guncellenmeli.
soc/software/gps_system/docs/RECEIVER_TIMING_AND_REACQUISITION.md
+1333
View File
File diff suppressed because it is too large Load Diff
soc/software/gps_system/header/gps/NavEquations.h
+13
View File
@@ -89,6 +89,19 @@
void topocent(const double* XPtr, const double* dxPtr, int size, t_phi_Lambda_h * phi_Lambda_h, t_topocentData* topcent);
void togeod(double a, double finv, double X, double Y, double Z, t_phi_Lambda_h * phi_Lambda_h, t_topocentData* topcent);
double tropo(double sinel, double hsta);
/*
* One measurement's standard deviation in metres: the URA the
* satellite broadcasts, the residual atmosphere implied by its
* elevation, and the code-tracking jitter implied by its
* carrier-to-noise density, combined in variance. This is what
* decides how far one satellite can move the fix, so it is
* exposed for host testing rather than kept file-static.
*/
double gps_nav_measurement_sigma_m(unsigned char ura_index,
double elevation_deg,
bool elevation_valid,
double cn0_dbhz,
bool cn0_valid);
double SquareRoot(double number);
void cart2geo(double X, double Y, double Z, int i, t_phi_Lambda_h * phi_Lambda_h);
int findUtmZone(double latitude, double longitude);
soc/software/gps_system/header/gps/gps_acq_manager.h
+29
View File
@@ -22,6 +22,20 @@ typedef struct {
uint64_t hot_retry_delay_ticks;
uint64_t hot_window_ticks;
unsigned int max_hot_streak;
/*
* Number of consecutive failed hot attempts served at the fast cadence
* before the long backoff applies. A satellite tracked seconds ago is
* almost certainly still visible, so that evidence must decay with
* repeated failures rather than be discarded on the first one.
*/
unsigned int hot_fast_attempts;
/*
* Retry cadence for PRNs the almanac predicts are below the elevation
* mask. They are slowed down, never excluded: the visibility model is
* advisory, and a stale almanac or a stale position must not be able to
* make a genuinely visible satellite unreachable.
*/
uint64_t unlikely_retry_delay_ticks;
} gps_acq_manager_config_t;
typedef struct {
@@ -30,6 +44,8 @@ typedef struct {
uint64_t hot_until_tick;
bool has_attempted;
bool attempt_was_hot;
/* Consecutive failed attempts since this PRN became hot. */
unsigned int hot_attempts;
} gps_acq_prn_entry_t;
typedef struct {
@@ -38,6 +54,8 @@ typedef struct {
unsigned int cold_cursor;
unsigned int hot_cursor;
unsigned int hot_streak;
/* Bit i set => PRN index i is predicted visible. All ones until known. */
uint32_t search_mask;
} gps_acq_manager_t;
void gps_acq_manager_init(
@@ -51,6 +69,17 @@ gps_acq_prn_state_t gps_acq_manager_state(
unsigned int prn);
const char* gps_acq_manager_state_name(gps_acq_prn_state_t state);
/*
* Publish the predicted-visible set. Bit i corresponds to PRN index i (the
* same 0-based index used everywhere in this API). Passing 0 restores the
* all-searchable default rather than silencing every PRN.
*/
void gps_acq_manager_set_search_mask(
gps_acq_manager_t* manager,
uint32_t search_mask);
uint32_t gps_acq_manager_search_mask(
const gps_acq_manager_t* manager);
void gps_acq_manager_mark_acquired(
gps_acq_manager_t* manager,
unsigned int prn);
soc/software/gps_system/header/gps/gps_almanac.h
+146
View File
@@ -0,0 +1,146 @@
#ifndef GPS_ALMANAC_H
#define GPS_ALMANAC_H
/*
* GPS LNAV almanac store and satellite-visibility model.
*
* The receiver already demodulates subframes 4 and 5 on every tracked
* channel; before this module the orbital elements in those pages were
* decoded far enough to identify the page and then discarded. That left the
* acquisition scheduler with no idea which satellites are above the horizon,
* so it swept all 32 PRNs blindly. Measured on the board, 75% of acquisition
* attempts were spent on satellites that were below the horizon and could
* never be found - with a single acquisition engine that directly multiplies
* the time to reacquire a real satellite.
*
* The almanac is deliberately advisory. Visibility narrows *how often* a PRN
* is searched, never whether it may be searched at all, so a stale almanac, a
* stale position or a decoding error can cost search efficiency but can never
* make a visible satellite permanently unreachable.
*/
#include <stdbool.h>
#include <stdint.h>
#define GPS_ALMANAC_PRN_COUNT 32u
/* Reference inclination for the LNAV almanac, IS-GPS-200 20.3.3.5.1.2. */
#define GPS_ALMANAC_I0_REF_SEMICIRCLES 0.30
/*
* How far an almanac may be propagated before it stops being evidence.
*
* This bound exists because the alternative is silent nonsense. A GPS week is
* 14.04 orbital periods, not 14, so aliasing the propagation time by one week
* moves the satellite 0.04 of an orbit - about 14 degrees of orbital angle,
* enough to swap "high in the sky" for "below the horizon". Anything that
* cannot be propagated inside this window therefore yields no prediction at
* all, which leaves the PRN fully searchable, rather than a confident wrong
* one that would demote a satellite that is actually up.
*
* Fourteen days is chosen to cover a realistic power-off: the persisted
* almanac has to survive a fortnight on the shelf and still aim the first
* cold-start sweep. Propagated correctly over that span the Keplerian
* almanac model is worth well under a degree of elevation, far inside the
* mask it feeds.
*/
#define GPS_ALMANAC_MAX_AGE_SEC 1209600.0
/*
* Largest week number that can be a GPS week. The receiver works on the
* 10-bit broadcast scale (0..1023) and full weeks are in the low thousands;
* 16383 is far past either and far below anything that would make a
* propagation interval meaningless.
*/
#define GPS_ALMANAC_WEEK_LIMIT 16383u
typedef struct {
bool valid;
unsigned char prn; /* 1..32 */
unsigned char health; /* 6-bit SV health, 0 = healthy */
double ecc;
double toa_sec;
double i0_rad;
double omega_dot_rps;
double sqrt_a;
double omega0_rad;
double omega_rad;
double m0_rad;
double af0_sec;
double af1_sec_per_sec;
} gps_almanac_entry_t;
typedef struct {
gps_almanac_entry_t sv[GPS_ALMANAC_PRN_COUNT];
/* Reference week/toa from subframe 5 page 25, when it has been seen. */
unsigned int week;
double toa_sec;
bool reference_valid;
} gps_almanac_t;
void gps_almanac_reset(gps_almanac_t* almanac);
/*
* Store one almanac page. subframe[] holds the ten 30-bit LNAV words with
* parity in the low six bits, the same representation parse_subframe() uses.
* Returns true when an almanac entry was decoded and stored.
*/
bool gps_almanac_store_page(gps_almanac_t* almanac,
const unsigned int* subframe,
unsigned char subframe_id);
/*
* Adopt the almanac reference week/toa. False when the pair is not a possible
* GPS reference, which is how a corrupt WNa lift or a flash record written by
* a build that had one is kept out of the propagation.
*/
bool gps_almanac_set_reference(gps_almanac_t* almanac,
unsigned int week,
double toa_sec);
unsigned int gps_almanac_valid_count(const gps_almanac_t* almanac);
/*
* The week an almanac entry must belong to if it is less than half a week old,
* for use before subframe 5 page 25 has supplied the real WNa. This is the
* only place that assumption is made; gps_almanac_satellite_ecef() treats the
* week it is given as fact.
*/
unsigned int gps_almanac_assumed_week(const gps_almanac_entry_t* entry,
unsigned int week,
double tow_sec);
/*
* ECEF position of one almanac satellite at (week, tow_sec). False when the
* entry is unusable or when almanac_week/toa place it further than
* GPS_ALMANAC_MAX_AGE_SEC from the requested time - an almanac that cannot be
* propagated must produce no answer rather than a wrong one.
*/
bool gps_almanac_satellite_ecef(const gps_almanac_entry_t* entry,
unsigned int almanac_week,
unsigned int week,
double tow_sec,
double ecef_out[3]);
bool gps_almanac_look_angles(const gps_almanac_entry_t* entry,
unsigned int almanac_week,
unsigned int week,
double tow_sec,
const double receiver_ecef_m[3],
double* elevation_deg_out,
double* azimuth_deg_out);
/*
* Bit i of *mask_out is set when PRN (i + 1) is predicted above
* mask_elevation_deg and healthy. Returns the number of satellites marked
* visible. With no usable almanac or receiver position the mask is left all
* ones so acquisition behaves exactly as it did before.
*/
unsigned int gps_almanac_visible_mask(const gps_almanac_t* almanac,
unsigned int week,
double tow_sec,
const double receiver_ecef_m[3],
double mask_elevation_deg,
uint32_t* mask_out);
#endif
soc/software/gps_system/header/gps/gps_ephemeris_cache.h
+56
View File
@@ -10,6 +10,12 @@ typedef struct {
gps_ephemeris_t ephemeris;
uint64_t committed_tick;
bool valid;
/*
* True while the entry came from flash and has not yet been confirmed by
* anything heard on the air. A restored record is a candidate, not an
* incumbent: it must never outrank a live broadcast.
*/
bool restored;
} gps_ephemeris_cache_entry_t;
typedef struct {
@@ -34,4 +40,54 @@ void gps_ephemeris_cache_invalidate(
gps_ephemeris_cache_t* cache,
unsigned int prn);
/*
* Store only when the record carries new information, so repeatedly decoding
* the same broadcast data set neither churns the cache nor produces a flash
* write. changed_out reports whether the stored content actually differs
* from what was already held; it is the flash-write trigger.
*/
bool gps_ephemeris_cache_store_if_new(
gps_ephemeris_cache_t* cache,
unsigned int prn,
const gps_ephemeris_t* ephemeris,
uint64_t now,
bool* changed_out);
/*
* Adopt a record read back from flash.
*
* Kept separate from gps_ephemeris_cache_store_if_new() because a persisted
* record has no known age relative to what is currently being broadcast, and
* the Toe comparison that orders two broadcast records cannot say so. Toe is
* a second-of-week, so gps_ephemeris_supersedes() resolves it with the signed
* half-week rule; that is exact between records from the same era and wrong in
* the wrong direction once the gap exceeds half a week. A receiver switched
* off from Friday to Tuesday would therefore rank its four-day-old flash
* record above the live broadcast and keep re-persisting the stale one for
* ever. Marking restored records instead lets the first broadcast for that
* PRN replace one outright, whatever the Toe arithmetic says.
*/
bool gps_ephemeris_cache_store_restored(
gps_ephemeris_cache_t* cache,
unsigned int prn,
const gps_ephemeris_t* ephemeris,
uint64_t now);
/*
* Restore a cached record and additionally qualify it against the current
* GPS time using the IS-GPS-200 curve fit rules. With time_known false only
* the structural rules apply, which is the boot case before any satellite has
* supplied authoritative time. A record rejected on age is dropped from the
* cache; a record that only fails the time check while time is unknown is
* kept as a candidate.
*/
bool gps_ephemeris_cache_restore_at(
gps_ephemeris_cache_t* cache,
unsigned int prn,
uint64_t now,
bool time_known,
int week,
double tow_sec,
gps_ephemeris_t* ephemeris_out);
#endif
soc/software/gps_system/header/gps/gps_ephemeris_store.h
+127
View File
@@ -0,0 +1,127 @@
#ifndef GPS_EPHEMERIS_STORE_H
#define GPS_EPHEMERIS_STORE_H
#include <stdbool.h>
#include <stdint.h>
#include "gps/gps_ip_defines.h"
/*
* Persistent ephemeris cache in SPI NOR flash.
*
* What is stored is the three raw LNAV subframes, not the parsed structure.
* That keeps the on-flash format independent of compiler layout and of any
* later change to gps_ephemeris_t, and it means a restored record is decoded by
* exactly the same parser the live receiver uses.
*
* Layout: two erase sectors used as an append-only log pair. Each sector opens
* with a header slot carrying a generation number; the sector with the higher
* valid generation is active. Records are appended one slot at a time and are
* committed by their CRC, so a record torn by power loss simply fails to
* validate and is skipped. When the active sector fills, the live set is
* rewritten into the spare sector at generation+1 and the old sector is erased,
* which is also the only time an erase happens.
*
* All flash access goes through the injected interface, so the whole module is
* exercised on the host against a simulated device.
*/
#define GPS_EPH_STORE_SLOT_SIZE 256u /* one flash page */
#define GPS_EPH_STORE_MAGIC 0x45504853u /* "EPHS" */
#define GPS_EPH_STORE_HEADER_MAGIC 0x45504844u /* "EPHD" */
#define GPS_EPH_STORE_VERSION 1u
typedef struct {
/* Reads never fail on NOR flash; erase/program report success. */
void (*read)(void* ctx, uint32_t address, void* dst, uint32_t len);
bool (*erase_sector)(void* ctx, uint32_t address);
bool (*program)(void* ctx, uint32_t address, const void* src,
uint32_t len);
void* ctx;
uint32_t base_address; /* first sector; must be erase aligned */
uint32_t sector_size; /* erase granularity */
} gps_ephemeris_store_flash_t;
typedef struct {
gps_ephemeris_store_flash_t flash;
bool mounted;
/*
* Latched when compaction cannot fit the live record set into one sector.
* Compaction erases the spare sector before it discovers this, so retrying
* would burn two erase cycles per attempt indefinitely. Persistence
* disables itself instead: losing the cache is recoverable, wearing the
* flash out is not. Unreachable in production (32 PRNs vs 255 slots); it
* exists because the slot cap makes it reachable, and because a future
* larger record would shrink the margin.
*/
bool capacity_exceeded;
unsigned int active_sector; /* 0 or 1 */
uint32_t generation;
uint32_t next_slot; /* next free slot index within the sector */
uint32_t slots_per_sector;
/* Highest sequence seen per PRN, so a stale duplicate never wins. */
uint32_t prn_sequence[GPS_PRN_COUNT];
bool prn_present[GPS_PRN_COUNT];
uint32_t sequence;
/* Diagnostics. */
uint32_t records_scanned;
uint32_t records_rejected;
uint32_t writes;
uint32_t erases;
uint32_t compactions;
} gps_ephemeris_store_t;
/* CRC-32 (IEEE 802.3, reflected) over the record body. Exposed for tests. */
uint32_t gps_ephemeris_store_crc32(const void* data, uint32_t len);
/*
* Scan both sectors and adopt the newer consistent one. Returns false only
* when the configuration is unusable; an empty or wholly corrupt store mounts
* successfully and simply holds nothing.
*/
bool gps_ephemeris_store_mount(
gps_ephemeris_store_t* store,
const gps_ephemeris_store_flash_t* flash);
/*
* Cap the usable slots per sector below what the sector physically holds.
* Compaction is otherwise only reachable after a full sector of records, which
* is impractical to reach on hardware; capping makes the same code path - real
* erases included - testable in seconds. Must be called after mount, and only
* shrinks. Zero means no cap.
*/
void gps_ephemeris_store_limit_slots(
gps_ephemeris_store_t* store,
uint32_t max_slots);
/*
* Rebuild every valid persisted ephemeris into the caller's handler. The
* subframes are re-parsed with the live decoder, and only records that pass the
* structural validity rules are delivered. Records are delivered newest-first
* per PRN.
*/
typedef void (*gps_ephemeris_store_load_fn)(
void* ctx, unsigned int prn, const gps_ephemeris_t* ephemeris);
unsigned int gps_ephemeris_store_load_all(
gps_ephemeris_store_t* store,
gps_ephemeris_store_load_fn handler,
void* ctx);
/*
* Persist one ephemeris, given the three raw subframes that produced it.
* Returns false on a flash failure. Writing is skipped, and true returned,
* when the record is not newer than what is already persisted - that is the
* wear policy, and it is why re-decoding the same broadcast costs nothing.
*/
bool gps_ephemeris_store_save(
gps_ephemeris_store_t* store,
unsigned int prn,
const gps_ephemeris_t* ephemeris,
const unsigned int subframe_words[3][10],
bool* wrote_out);
/* Erase both sectors and start an empty generation. */
bool gps_ephemeris_store_format(gps_ephemeris_store_t* store);
#endif
soc/software/gps_system/header/gps/gps_ephemeris_validity.h
+98
View File
@@ -0,0 +1,98 @@
#ifndef GPS_EPHEMERIS_VALIDITY_H
#define GPS_EPHEMERIS_VALIDITY_H
#include <stdbool.h>
#include <stdint.h>
#include "gps/gps_ip_defines.h"
/*
* IS-GPS-200 ephemeris validity rules, kept separate from the decoder so a
* cached (RAM or flash) record can be re-qualified against the current
* receiver time without re-decoding subframes 1..3.
*
* Age is always evaluated as a signed half-week difference between the
* requested GPS time and Toe. That keeps the check correct across the
* Saturday/Sunday second-of-week rollover without needing a week number for
* Toe, which LNAV does not broadcast.
*/
/*
* Curve fit interval implied by the subframe 2 fit flag and the IODC
* (IS-GPS-200 Table 20-XI / 20-XII). The returned value is the full
* interval; the usable window is +/- half of it around Toe.
*
* Project assumption: only the IODC ranges the ICD tabulates explicitly are
* encoded. A fit flag of one with an IODC outside those ranges falls back to
* GPS_EPHEMERIS_LONG_FIT_FALLBACK_HOURS, which is shorter than the ICD's
* smallest tabulated long fit (8 hours) and therefore conservative.
*/
#define GPS_EPHEMERIS_NOMINAL_FIT_HOURS 4.0
#define GPS_EPHEMERIS_LONG_FIT_FALLBACK_HOURS 6.0
double gps_ephemeris_fit_interval_hours(unsigned int iodc, bool fit_flag);
/* Half of the curve fit interval, in seconds: the maximum |t - Toe|. */
double gps_ephemeris_max_toe_offset_sec(const gps_ephemeris_t* eph);
/*
* Signed seconds from Toe to (week, tow_sec), wrapped into +/- half a week.
* Returns false when the inputs are not finite or Toe is out of range.
*/
bool gps_ephemeris_toe_offset_sec(
const gps_ephemeris_t* eph,
int week,
double tow_sec,
double* offset_sec_out);
typedef enum {
GPS_EPHEMERIS_OK = 0,
GPS_EPHEMERIS_REJECT_NULL,
GPS_EPHEMERIS_REJECT_UNCOMMITTED,
GPS_EPHEMERIS_REJECT_PRN,
GPS_EPHEMERIS_REJECT_IOD,
GPS_EPHEMERIS_REJECT_HEALTH,
GPS_EPHEMERIS_REJECT_ACCURACY,
GPS_EPHEMERIS_REJECT_PARAMETERS,
GPS_EPHEMERIS_REJECT_AGE
} gps_ephemeris_status_t;
const char* gps_ephemeris_status_name(gps_ephemeris_status_t status);
/*
* Structural qualification: committed flag, PRN identity, IODE/IODC
* consistency, health, URA index and orbital parameter sanity. This is the
* part of the check that does not need receiver time, so it can run at boot
* on a flash record before any satellite has been decoded.
*/
gps_ephemeris_status_t gps_ephemeris_check_static(
const gps_ephemeris_t* eph,
unsigned int expected_prn);
/*
* Full qualification. When time_known is false only the static rules run and
* the record is reported as a candidate; the caller must not navigate on it
* until receiver time exists.
*/
gps_ephemeris_status_t gps_ephemeris_check(
const gps_ephemeris_t* eph,
unsigned int expected_prn,
bool time_known,
int week,
double tow_sec);
/*
* True when candidate should replace current in the cache. A record only
* supersedes another when it carries different issue-of-data or a strictly
* newer Toe, so decoding the same ephemeris again never triggers a rewrite.
*/
bool gps_ephemeris_supersedes(
const gps_ephemeris_t* candidate,
const gps_ephemeris_t* current);
/* True when the two records describe the same broadcast data set. */
bool gps_ephemeris_same_issue(
const gps_ephemeris_t* first,
const gps_ephemeris_t* second);
#endif
soc/software/gps_system/header/gps/gps_interrupt_handlers.h
+69
View File
@@ -10,7 +10,17 @@
uint32_t plic_repeat_total;
uint32_t nav_epoch_coalesced_total;
uint32_t pps_sequence_discontinuity_total;
/*
* Historic counter, kept for continuity: it conflates two unrelated
* events and misled three separate diagnoses. Use the two below.
*/
uint32_t nav_epoch_incomplete_total;
/* The epoch assembly genuinely failed to gather its expected set. */
uint32_t nav_epoch_assembly_failed_total;
/* The epoch assembled correctly but carried fewer than
* GPS_NAV_MIN_WLS_SATELLITES, so no solution was possible. This is a
* satellite-availability statement, not an epoch fault. */
uint32_t nav_epoch_below_minimum_total;
uint32_t nav_epoch_snapshot_overwrite_total;
uint32_t how_anchor_rejected_total;
uint32_t measurement_missed_total;
@@ -19,6 +29,62 @@
uint32_t satellite_lost_total;
uint32_t ephemeris_cache_store_total;
uint32_t ephemeris_cache_restore_total;
uint32_t ephemeris_cache_miss_total;
uint32_t ephemeris_rejected_total;
uint32_t warm_seed_armed_total;
uint32_t warm_seed_confirmed_total;
uint32_t warm_seed_rejected_total;
/* Split of warm_seed_rejected_total by cause: the tracking IP declined
* the armed seed (channel lost lock between the check and the arm), or
* the first seeded epoch failed the nav-solve grid snap (PRN context no
* longer describes this satellite). */
uint32_t warm_seed_refused_total;
uint32_t warm_seed_snap_failed_total;
/* Broadcast issue-of-data differs from the ephemeris currently in use,
* i.e. the in-use record (often flash restored) has been superseded. */
uint32_t ephemeris_iod_mismatch_total;
/*
* Why an epoch was discarded, not just that it was. expected_mask is
* latched at epoch start; any channel in it that never reports holds the
* epoch to its deadline. missing/blocked name those channels.
*/
uint32_t nav_epoch_last_expected_mask;
uint32_t nav_epoch_last_missing_mask;
uint32_t nav_epoch_blocked_mask;
uint32_t nav_epoch_silenced_total;
uint32_t nav_epoch_reseed_total;
/*
* Why epochs are discarded. deadline vs restart are different faults:
* deadline means an expected channel never reported inside the
* collection window, restart means a newer epoch's event arrived first.
*/
uint32_t nav_epoch_deadline_discard_total;
uint32_t nav_epoch_restart_discard_total;
/*
* Capture-counter spread of a COMPLETE epoch, in PPS ticks (125 MHz).
* This is the real inter-satellite arrival spread and is what the
* 25 ms sync window has to accommodate; transit time alone varies by
* ~18 ms between a zenith and a horizon satellite. Only the firmware
* can measure it: the epoch exists for ~30 ms in every 200 ms, so an
* external debugger cannot sample it.
*/
uint32_t nav_epoch_spread_last_ticks;
uint32_t nav_epoch_spread_max_ticks;
/* Per-channel count of "was expected but did not report". */
uint32_t nav_epoch_missing_count[GPS_TRCK_IP_COUNT];
/* Warm reacquisition latency, CLINT-resolution (2 us on Genesys2).
* lock_to_epoch isolates what the seed changed: tracking start to the
* first accepted navigation epoch. drop_to_epoch adds the acquisition
* search, which dominates and is unrelated to the seed. */
uint32_t warm_seed_lock_to_epoch_us;
uint32_t warm_seed_drop_to_epoch_us;
uint32_t warm_seed_lock_to_epoch_min_us;
uint32_t warm_seed_lock_to_epoch_max_us;
uint32_t flash_ephemeris_loaded_total;
uint32_t flash_ephemeris_write_total;
uint32_t flash_ephemeris_error_total;
uint32_t last_ephemeris_reject_prn;
uint32_t last_ephemeris_reject_reason;
uint32_t last_connected_prn;
uint32_t last_lost_prn;
uint64_t last_connected_tick;
@@ -55,6 +121,9 @@
void gps_background_task(void);
bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot);
void gps_get_runtime_diagnostics(gps_runtime_diagnostics_t* diagnostics);
/* Records why a decoded or cached ephemeris was refused by the solver.
* reason carries a gps_ephemeris_status_t value. */
void gps_report_ephemeris_rejection(unsigned int prn, unsigned int reason);
void gps_tracking_set_eskf_doppler_alpha(double alpha);
double gps_tracking_get_eskf_doppler_alpha(void);
void trck_intr_handler(volatile gps_trck_t* const trck_ip, volatile gps_frame_t *gps_frame_parsed, volatile int unsigned gps_subframe[10], unsigned char trck_ip_no);
soc/software/gps_system/header/gps/gps_ip_defines.h
+190 -2
View File
@@ -8,6 +8,7 @@
#include <bsp/soc.h>
#include "gps/matrixLib.h"
#include "gps/gps_macros.h"
#include "gps/gps_almanac.h"
#ifdef __riscv
#define GPS_IRQ_HANDLER_ATTR __attribute__((interrupt("machine"), aligned(4)))
@@ -45,7 +46,12 @@
#define GPS_ACQ_IP_COUNT ACQ_ENGINE_COUNT
#define GPS_ACQ_IP_BASE_ADDR(index) (BSP_GPS_ACQ_BASE + ((uintptr_t)(index) * BSP_GPS_ACQ_STRIDE))
#define GPS_ACQ_INTR_LINE(index) (BSP_GPS_ACQ_IRQ + ((unsigned int)(index) * BSP_GPS_ACQ_IRQ_STRIDE))
#define GPS_EVENT_QUEUE_SIZE 64u
/* Sized for the worst foreground stall, which is now a flash
* compaction sector erase (hundreds of ms). At 9 channels the
* 200 ms nav-solve events alone contribute ~23 entries during such
* a stall; 64 left no headroom for a concurrent burst and the
* startup burst was already overflowing. */
#define GPS_EVENT_QUEUE_SIZE 128u
/* Genesys2 CLINT is 500 kHz. Keep foreground maintenance at
* 10 Hz; the tracking IP's 20 ms NAV timer is independent. */
#define GPS_SYSTICK_HZ 10u
@@ -61,12 +67,66 @@
* is calibrated. The live stationary test produced a 22.7% exclusion
* rate and substantially worse position dispersion. */
#define GPS_NAV_ENABLE_FDE 0u
/*
* Measurement weighting. The elevation term is 3 m at zenith
* mapped by 1/sin(elevation); the floor bounds it where 1/sin
* diverges, because a zero weight is a failed epoch rather than a
* discounted satellite. Three degrees leaves the term at 57 m -
* some 350 times lighter than zenith - and, unlike the ten degree
* floor it replaces, lets the mapping actually work across the
* range where a satellite carries most of the error.
*/
#define GPS_NAV_WEIGHT_MIN_ELEVATION_DEG 3.0
/*
* Charged on top when a satellite is at or below the horizon, so
* the extrapolated atmospheric correction and the reflection it
* probably arrived by are paid for in variance rather than
* assumed away.
*/
#define GPS_NAV_BELOW_HORIZON_SIGMA_M 50.0
/* Upper bound on the carrier-to-noise term, before calibration. */
#define GPS_NAV_CN0_SIGMA_MAX_M 30.0
#define GPS_PROFILE_CPU_HZ 50000000ULL
#define GPS_ACQ_RETRY_DELAY_TICKS (GPS_SYSTICK_HZ / 2u)
#define GPS_ACQ_HOT_RETRY_DELAY_TICKS (GPS_SYSTICK_HZ * 30u)
#define GPS_ACQ_REACQ_PRIORITY_TICKS \
(GPS_SYSTICK_HZ * 2u * 60u * 60u)
#define GPS_ACQ_MAX_HOT_STREAK 4u
/*
* Fast retries granted to a freshly lost satellite before the long
* hot backoff applies. Six attempts at the 0.5 s cold cadence is
* ~3 s, comfortably longer than the tracking loop needs to settle
* after a signal interruption, and far below the 30 s backoff that
* previously applied from the first retry.
*/
#define GPS_ACQ_HOT_FAST_ATTEMPTS 6u
/*
* Almanac-directed acquisition. A PRN predicted below the mask
* is retried on this slow cadence instead of the 0.5 s sweep, so
* the single acquisition engine spends its time on satellites
* that are actually up. It is a demotion, not an exclusion: the
* PRN is still revisited so a stale almanac or a stale position
* self-corrects, and a satellite that has just risen is found
* without waiting for the next almanac refresh.
*/
#define GPS_ACQ_UNLIKELY_RETRY_DELAY_TICKS (GPS_SYSTICK_HZ * 20u)
#define GPS_ACQ_VISIBILITY_REFRESH_TICKS (GPS_SYSTICK_HZ * 10u)
/*
* Search a little below the horizon: almanac elevation carries a
* few degrees of error when the data is days old, and a satellite
* about to rise is worth finding early.
*/
#define GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG (-5.0)
/*
* The same mask while the receiver position is still the seed
* read back from flash rather than a fix taken this run. The
* seed says where the receiver was; nothing yet says it has not
* been moved since. A displacement of D km moves a look angle by
* roughly D/111 degrees, so -25 tolerates a couple of thousand
* kilometres of unknown migration while still ruling out the far
* side of the earth.
*/
#define GPS_ACQ_VISIBILITY_SEED_MASK_ELEVATION_DEG (-25.0)
#define GPS_ACQ_STUCK_WARN_TICKS (GPS_SYSTICK_HZ * 8u)
#define GPS_EPHEMERIS_CACHE_MAX_AGE_TICKS \
(GPS_SYSTICK_HZ * 2u * 60u * 60u)
@@ -79,6 +139,83 @@
#define GPS_HOW_ANCHOR_MAX_AGE_SEC 12.0
#define GPS_HOW_CONTINUITY_MAX_GAP_SEC 120.0
#define GPS_HOW_CONTINUITY_TOLERANCE_SEC 0.050
/* Warm/hot reacquisition phase seeding. The PRN context is only
* reused while the receiver clock prediction stays far inside one
* 20 ms navigation bit; at a few ppm the 15 minute bound leaves
* roughly two orders of magnitude of margin, and broadcast
* ephemeris validity caps reuse well before that anyway. */
/* Persistent ephemeris cache. The Genesys2 image places the
* bitstream at 0 and the application at 0x01800000 on a 32 MiB
* S25FL256S, so the last two 64 KiB sectors are free. Keeping the
* store at the very top leaves the application region free to grow. */
#ifndef GPS_EPHEMERIS_FLASH_ENABLE
#define GPS_EPHEMERIS_FLASH_ENABLE 1
#endif
/* Test hooks. MAX_SLOTS caps the usable slots per sector so
* compaction can be exercised on real hardware without writing 256
* records; 0 means use the whole sector. FORCE_FORMAT erases both
* sectors at boot so the format path can be observed. */
#ifndef GPS_EPHEMERIS_STORE_MAX_SLOTS
#define GPS_EPHEMERIS_STORE_MAX_SLOTS 0u
#endif
#ifndef GPS_EPHEMERIS_FLASH_FORCE_FORMAT
#define GPS_EPHEMERIS_FLASH_FORCE_FORMAT 0
#endif
#ifndef GPS_EPHEMERIS_FLASH_BASE
#define GPS_EPHEMERIS_FLASH_BASE 0x01FE0000UL
#endif
/*
* Persistent navigation context (almanac, ionosphere model, last
* position) in the two 64 KiB sectors immediately below the
* ephemeris store. The application image lives at 0x01800000 and
* is far smaller than the gap.
*/
#ifndef GPS_NAV_CONTEXT_FLASH_BASE
#define GPS_NAV_CONTEXT_FLASH_BASE 0x01FC0000UL
#endif
#ifndef GPS_NAV_CONTEXT_ENABLE
#define GPS_NAV_CONTEXT_ENABLE 1
#endif
/*
* How often the context is rebuilt and offered to the store. The
* store compares a CRC first, so a save that would not change the
* bytes never reaches the flash; this period only bounds how
* quickly a genuine change is captured.
*/
#define GPS_NAV_CONTEXT_SAVE_PERIOD_TICKS (GPS_SYSTICK_HZ * 60u)
/*
* What counts as the receiver having moved, for the purpose of
* the persisted position seed, and how often that seed may be
* refreshed. The seed only aims a visibility prediction and a
* look angle changes by ~0.003 degrees per kilometre of receiver
* error, so 5 km is far inside what the mask can notice. The two
* limits together are what keep the erase count tied to real news
* rather than to the save timer: a stationary receiver offers
* byte-identical payloads for ever and never touches the flash.
*/
#define GPS_NAV_CONTEXT_POSITION_EPSILON_M 5000.0
#define GPS_NAV_CONTEXT_POSITION_REFRESH_TICKS \
(GPS_SYSTICK_HZ * 60u * 60u)
#ifndef GPS_WARM_SEED_ENABLE
#define GPS_WARM_SEED_ENABLE 1
#endif
#define GPS_NAV_WARM_SEED_MAX_AGE_SEC 900.0
/* Never arm within this distance of a navigation-bit boundary, so
* the seed cannot be applied one bit late. */
#define GPS_NAV_WARM_SEED_GUARD_SEC 0.004
/* A seeded epoch must land on the known nav-solve grid within this
* tolerance before its time anchor is trusted. */
#define GPS_NAV_WARM_SEED_SNAP_SEC 0.050
/*
* A channel is only expected to contribute to an epoch while it is
* actually producing nav-solve events. busy+locked+IRQ-enabled is
* not the same thing: the RTL timer returns to IDLE whenever
* trck_busy drops and will not restart without a subframe or a
* seed, so a channel that dips and regains lock looks ready while
* emitting nothing. Left in the expected set it holds every epoch
* to its deadline until its next subframe, up to six seconds.
*/
#define GPS_NAV_SILENT_CHANNEL_EPOCHS 2u
#if (GPS_NAV_SOLVE_TIMER_TICKS == 0u)
#error "GPS_NAV_SOLVE_TIMER_TICKS must be non-zero"
@@ -143,6 +280,9 @@
#define GPS_TRCK_NAV_SOLVE_FLAG_EN_POS 12 // (Default 1)
#define GPS_TRCK_MEASUREMENT_INTR_EN_POS 13
#define GPS_TRCK_MEASUREMENT_FLAG_EN_POS 14
/* Arms NAV_SOLVE_PHASE_SEED. Hardware answers and clears this bit, so it
* reads back zero once the timer has evaluated the request. */
#define GPS_TRCK_NAV_SOLVE_SEED_ARM_POS 15
// acq ip status reg bits.
@@ -158,6 +298,8 @@
#define GPS_TRCK_STOP_UNLOCK_POS 5
#define GPS_TRCK_NAV_SOLVE_FLAG_POS 6
#define GPS_TRCK_MEASUREMENT_FLAG_POS 7
/* NAV solve epochs are running on a firmware seed no subframe has confirmed. */
#define GPS_TRCK_NAV_SOLVE_PROVISIONAL_POS 8
#define GPS_TRCK_MEAS_META_VALID_POS 0
#define GPS_TRCK_MEAS_META_NAV_BIT_POS 1
@@ -261,6 +403,7 @@
int32_t MEASUREMENT_SUM_Q; // 'h68
uint32_t MEASUREMENT_WBP_LO; // 'h6c
uint32_t MEASUREMENT_WBP_HI; // 'h70
uint32_t NAV_SOLVE_PHASE_SEED; // 'h74
}gps_trck_t;
typedef enum {
@@ -292,6 +435,7 @@
_Static_assert(offsetof(gps_trck_t, MEASUREMENT_META) == 0x50, "gps_trck_t measurement meta offset mismatch");
_Static_assert(offsetof(gps_trck_t, MEASUREMENT_CARRIER_Q7) == 0x58, "gps_trck_t measurement carrier offset mismatch");
_Static_assert(offsetof(gps_trck_t, MEASUREMENT_WBP_HI) == 0x70, "gps_trck_t measurement WBP offset mismatch");
_Static_assert(offsetof(gps_trck_t, NAV_SOLVE_PHASE_SEED) == 0x74, "gps_trck_t nav solve phase seed offset mismatch");
#endif
@@ -333,11 +477,20 @@
double tgd; // from subframe 1
gpstime_t tow; // tow value of last reached subframe, calculated from TOW in Handover Word.
gpstime_t toc; // from subframe 1
gpstime_t toe; // from subframe 2
gpstime_t toe; // from subframe 2; .week resolved at commit
/*
* LNAV broadcasts Toe as a second-of-week only, so toe.week has to be
* reconstructed from the subframe 1 week number and the handover word
* of the subframe that carried it. Without it the curve-fit age check
* can only work modulo one week, and a record exactly N weeks old
* reads as brand new. False until that reconstruction has happened.
*/
bool toe_week_valid;
double af0; // from subframe 1
double af1; // from subframe 1
double af2; // from subframe 1
unsigned char iode; // from subframe 2
bool fit_flag; // from subframe 2 word 10: curve fit interval > 4 h.
double crs; // from subframe 2
double deltan; // from subframe 2
double m0; // from subframe 2
@@ -460,6 +613,10 @@
void trck_disable_nav_solve_flag(volatile gps_trck_t* trck_ip); // disables tracking ip nav solve flag generation.
void trck_enable_nav_solve_flag(volatile gps_trck_t* trck_ip); // disables tracking ip nav solve flag generation.
void trck_enable_nav_solve_intr(volatile gps_trck_t* trck_ip); // enables tracking ip nav solve interrupt generation.
void trck_arm_nav_solve_phase_seed(volatile gps_trck_t* trck_ip,
uint32_t phase_seed); // warm-start navigation-bit phase for nav solve epochs.
bool trck_nav_solve_provisional(volatile gps_trck_t* trck_ip); // epochs run on an unconfirmed firmware seed.
bool trck_nav_solve_seed_pending(volatile gps_trck_t* trck_ip); // arm bit still set; hardware has not answered yet.
void trck_disable_nav_solve_intr(volatile gps_trck_t* trck_ip); // enables tracking ip nav solve interrupt generation.
void trck_disable_stop_on_unlock(volatile gps_trck_t* trck_ip); // enable functionality.
@@ -505,6 +662,37 @@
#endif
}
bool gps_get_ionoutc(ionoutc_t* ionoutc);
void gps_lnav_reset_ionoutc(void);
/*
* Receiver-global almanac collected from subframe 4/5 pages on every
* tracked channel. Used to predict which PRNs are above the horizon
* so acquisition does not sweep the whole constellation blindly.
*/
const gps_almanac_t* gps_lnav_get_almanac(void);
void gps_lnav_reset_almanac(void);
/*
* Raw LNAV words for the pages worth persisting, and the matching
* restore entry points. Restoring re-parses through the live decoder,
* so a corrupt or stale flash record is rejected exactly as a bad
* broadcast would be.
*/
bool gps_lnav_get_iono_page(uint32_t words_out[10]);
uint32_t gps_lnav_get_almanac_pages(
uint32_t words_out[GPS_ALMANAC_PRN_COUNT][10]);
bool gps_lnav_restore_iono_page(const unsigned int* subframe);
bool gps_lnav_restore_almanac_page(const unsigned int* subframe,
unsigned char subframe_id);
void gps_lnav_restore_almanac_reference(unsigned int week,
double toa_sec);
/*
* Last converged WLS receiver position, for consumers that need a
* coarse position rather than a fix - the almanac visibility model in
* particular. False until the first solution.
*/
bool gps_nav_last_receiver_ecef(double ecef_out[3]);
bool gps_lnav_broadcast_iode(const unsigned int* subframe,
unsigned char subframe_id,
unsigned char* iode_out);
void gps_background_task(void);
void gps_process_isr_events(void);
void gps_acq_scheduler_run(void);
soc/software/gps_system/header/gps/gps_nav_context_store.h
+127
View File
@@ -0,0 +1,127 @@
#ifndef GPS_NAV_CONTEXT_STORE_H
#define GPS_NAV_CONTEXT_STORE_H
#include <stdbool.h>
#include <stdint.h>
#include "gps/gps_almanac.h"
#include "gps/gps_ephemeris_store.h"
/*
* Persistent navigation context: almanac, ionosphere/UTC model, and the last
* known receiver position.
*
* These three share one store because they share one lifetime and one purpose.
* Unlike ephemeris, none of them is per-channel and none expires in hours:
*
* - The almanac stays usable for weeks and tells the acquisition scheduler
* which satellites are above the horizon. Collected from the air it takes
* 12.5 minutes, because the 25 pages come round one per 30 s frame, so
* without persistence every reset sweeps the whole constellation blind for
* that entire window.
* - The ionosphere/UTC page (subframe 4 page 18) arrives on the same 12.5
* minute cycle, and without it every pseudorange carries an uncorrected
* ionospheric delay.
* - The last position is what turns an almanac into a visibility prediction.
*
* Shape follows use. This is a small blob rewritten rarely and in full, so it
* is stored as an A/B pair of whole sectors rather than through the ephemeris
* store's append log: the log is keyed per PRN and compacts, which buys
* nothing here and costs the complexity that produced a destructive bug once
* already. A save that would not change the bytes does not touch the flash at
* all, so the erase count follows how often the constellation actually
* publishes new data - roughly daily - not how often the receiver boots.
*
* Like the ephemeris store, what is persisted is the raw LNAV words rather
* than parsed structures, so the on-flash format is independent of compiler
* layout and a restored page is decoded by exactly the live decoder.
*/
#define GPS_NAV_CONTEXT_MAGIC 0x4E435458u /* "NCTX" */
#define GPS_NAV_CONTEXT_VERSION 1u
#define GPS_NAV_CONTEXT_WORDS 10u
typedef struct {
/* Raw LNAV almanac page per PRN; entry absent when its mask bit is 0. */
uint32_t almanac_words[GPS_ALMANAC_PRN_COUNT][GPS_NAV_CONTEXT_WORDS];
uint32_t almanac_present_mask;
/* Subframe 5 page 25 reference, when it has been seen. */
uint32_t almanac_week;
uint32_t almanac_toa_sec;
/* Raw subframe 4 page 18 (ionosphere/UTC). */
uint32_t iono_words[GPS_NAV_CONTEXT_WORDS];
/* Last converged WLS position and the receiver time it was taken at. */
double position_ecef_m[3];
double position_tow_sec;
uint32_t position_week;
uint8_t almanac_reference_valid;
uint8_t iono_valid;
uint8_t position_valid;
uint8_t reserved;
} gps_nav_context_payload_t;
typedef struct {
gps_ephemeris_store_flash_t flash;
bool mounted;
bool has_payload;
unsigned int active_sector; /* 0 or 1; the one holding the live copy */
uint32_t generation;
uint32_t payload_crc32; /* of the active payload, for the wear policy */
/* Diagnostics. */
uint32_t writes;
uint32_t erases;
uint32_t skipped_writes;
uint32_t rejected;
} gps_nav_context_store_t;
/*
* Adopt the newer of the two sectors that carries a self-consistent payload.
* An empty or wholly corrupt store still mounts; it simply holds nothing.
*/
bool gps_nav_context_store_mount(
gps_nav_context_store_t* store,
const gps_ephemeris_store_flash_t* flash);
/* Read the persisted payload. False when nothing valid is stored. */
bool gps_nav_context_store_load(
const gps_nav_context_store_t* store,
gps_nav_context_payload_t* payload_out);
/*
* Persist the payload, writing only when the bytes would actually change.
* *wrote_out reports whether flash was touched.
*
* The comparison is a CRC over the whole struct, padding included, so the
* caller must zero the payload before filling it. A caller that does not
* would feed indeterminate padding into the CRC, every save would look like a
* change, and the wear policy would silently stop working.
*/
bool gps_nav_context_store_save(
gps_nav_context_store_t* store,
const gps_nav_context_payload_t* payload,
bool* wrote_out);
bool gps_nav_context_store_format(gps_nav_context_store_t* store);
/*
* Is a new position worth rewriting the context for?
*
* The wear policy above only works if the bytes offered are actually stable
* while nothing meaningful has changed, and a live WLS position is not: it
* moves by metres every epoch, so feeding it in unfiltered makes every
* periodic save a fresh CRC and therefore an erase. At a 60 s period that is
* 1440 erase cycles a day on a two-sector pair - months, not years, of endurance
* - to persist a seed that is only ever read once, at the next boot.
*
* The seed's only job is to turn an almanac into a visibility prediction, and
* a look angle at 20200 km altitude moves by about 0.003 degrees per kilometre
* of receiver error. Kilometres are therefore free and metres are noise, so
* the question this answers is "has the receiver moved far enough that the
* stored seed would aim the next cold start differently", not "has the
* position changed".
*/
bool gps_nav_context_position_changed(const double persisted_ecef_m[3],
const double candidate_ecef_m[3],
double epsilon_m);
#endif
soc/software/gps_system/header/gps/gps_nav_epoch.h
+21
View File
@@ -76,6 +76,27 @@ bool gps_nav_epoch_validate_complete(
double tow_tolerance_sec,
gps_nav_epoch_selection_t* selection);
/*
* Lift one channel's corrected sample onto the same continuous axis as the
* epoch's reference sample.
*
* The RF sample counter is 32 bits at 16.368 MHz, so it wraps every 262
* seconds - roughly every four and a half minutes of operation, in the middle
* of normal tracking. Every accepted member of an epoch lies inside the
* coherence window, so its signed modulo-2^32 distance from the reference is
* unambiguous; taking that distance and re-adding it to the reference produces
* a set that is internally consistent even when some channels have wrapped and
* others have not. Pseudoranges are formed as differences against the
* reference, so the absolute value never has to be continuous - only the set
* does.
*
* Exposed rather than kept file-static because this is the one piece of
* arithmetic standing between a counter wrap and every pseudorange in the fix.
*/
double gps_nav_epoch_unwrap_sample(uint32_t raw_sample,
double corrected_sample,
uint32_t reference_sample);
/*
* Validate a newest-only hardware mailbox after one or more capture events
* were coalesced. sequence_gap receives the number of real elapsed epochs;
soc/software/gps_system/header/gps/gps_pps.h
+2
View File
@@ -26,6 +26,8 @@ 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);
/* Coherent 64-bit read of the free-running PPS-domain counter. */
uint64_t gps_pps_read_raw_counter(void);
gps_pps_time_filter_result_t gps_pps_filter_clock_bias(
uint64_t epoch_counter,
double measured_clock_bias_m,
soc/software/gps_system/header/gps/gps_time_anchor.h
+42
View File
@@ -70,6 +70,48 @@ bool gps_time_correct_capture_counter(
uint64_t* corrected_counter_out,
double* applied_correction_sec_out);
/*
* Navigation-bit phase for the tracking IP's NAV_SOLVE_PHASE_SEED register.
*
* The seed is the counter value in effect for the next navigation bit after
* the timer consumes it, and the hardware raises an epoch on the bit whose
* counter value is nav_solve_ticks - 1. So the seed is the absolute
* navigation-bit index of that next bit, modulo nav_solve_ticks, which
* reproduces exactly the grid the broadcast HOW Z-count would have produced.
*
* elapsed_sec is receiver time since the anchor, measured with the free-running
* PPS counter. That counter is 64-bit and never wraps in practice, unlike the
* 32-bit RF sample counter which repeats every ~262 s, so it is the only
* timebase that spans a multi-minute outage unambiguously.
*
* The request is refused when the predicted position inside the current
* navigation bit is within guard_sec of either boundary, so the seed can never
* be applied one bit late. A refused request simply means "retry later".
*/
bool gps_nav_bit_phase_from_elapsed(
double anchor_tow_sec,
double elapsed_sec,
double bit_period_sec,
uint32_t nav_solve_ticks,
double guard_sec,
uint32_t* phase_seed_out);
/*
* Transmit time of a seeded channel's first navigation-solve epoch.
*
* The hardware can only raise that epoch on a multiple of the nav-solve period,
* so snapping the prediction to that grid recovers the exact time and, at the
* same time, proves the anchor is still valid: a prediction further than
* max_snap_sec from the grid means the context no longer describes this
* satellite. The result may exceed one week; the caller normalises it.
*/
bool gps_nav_epoch_snap_from_elapsed(
double anchor_tow_sec,
double elapsed_sec,
double period_sec,
double max_snap_sec,
double* snapped_tow_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
+120 -23
View File
@@ -301,14 +301,40 @@ static bool least_squares_ura_sigma_m(unsigned char ura_index, double* sigma_m)
return true;
}
static double least_squares_measurement_weight(unsigned char ura_index,
double elevation_deg,
bool elevation_valid,
double cn0_dbhz,
bool cn0_valid)
/*
* Elevation at which the atmospheric models are evaluated.
*
* A satellite is tracked past the horizon - it sets, or it arrives by
* reflection - and both corrections used to switch themselves off there:
* tropo() was called only for elv > 0 and klobuchar_delay_meters() returned
* zero for elevation <= 0. So a satellite crossing the horizon lost about 30
* metres of correction in one step, and that whole delay stayed in its
* measurement as an uncorrected bias.
*
* Evaluating both at the horizon instead is an extrapolation, and it
* under-corrects, because the true path below the horizon is longer still. It
* is a far better estimate than zero, and it makes the measurement model
* continuous, which matters more: a discontinuity means the same satellite
* changes the fix by tens of metres between two consecutive epochs for no
* physical reason. The extra uncertainty of extrapolating is charged in
* gps_nav_measurement_sigma_m(), which keeps degrading below the horizon.
*/
static double least_squares_atmosphere_elevation_deg(double elevation_deg)
{
if(!isfinite(elevation_deg))
{
return 0.0;
}
return (elevation_deg < 0.0) ? 0.0 : elevation_deg;
}
double gps_nav_measurement_sigma_m(unsigned char ura_index,
double elevation_deg,
bool elevation_valid,
double cn0_dbhz,
bool cn0_valid)
{
const double dtr = 0.01745329251994329576923690768489;
const double min_elevation_deg = 10.0;
const double elevation_sigma_zenith_m = 3.0;
double ura_sigma_m;
if(!least_squares_ura_sigma_m(ura_index, &ura_sigma_m))
@@ -319,11 +345,31 @@ static double least_squares_measurement_weight(unsigned char ura_index,
double variance_m2 = ura_sigma_m * ura_sigma_m;
if(elevation_valid)
{
/*
* 3 m at zenith mapped by 1/sin(elevation), which is the residual
* atmosphere a single-frequency receiver cannot model.
*
* The floor used to be 10 degrees, which capped this term at 17.3 m
* and so silently switched the mapping off exactly where it matters:
* at 5 degrees the formula's own answer is 34.4 m and at 2 degrees it
* is 86 m, so a satellite low enough to be carrying most of the error
* was being trusted 4 to 25 times more than this same model said it
* deserved. Nothing was excluded by that floor - it only made the
* weighting stop working.
*
* The floor is now low enough for the mapping to do its job and no
* lower: 1/sin diverges at the horizon, and a zero weight is not a
* down-weighted satellite but a failed epoch, because
* least_squares_build_postfit() refuses a non-positive weight. Below
* the floor the term is held at its floor value, which still leaves
* such a satellite roughly 350 times lighter than one at zenith.
*/
double bounded_elevation_deg = elevation_deg;
if(!isfinite(bounded_elevation_deg) ||
(bounded_elevation_deg < min_elevation_deg))
(bounded_elevation_deg < GPS_NAV_WEIGHT_MIN_ELEVATION_DEG))
{
bounded_elevation_deg = min_elevation_deg;
/* A non-finite elevation is unknown, not good: take the floor. */
bounded_elevation_deg = GPS_NAV_WEIGHT_MIN_ELEVATION_DEG;
}
if(bounded_elevation_deg > 90.0)
{
@@ -332,13 +378,28 @@ static double least_squares_measurement_weight(unsigned char ura_index,
double sin_el = sin(bounded_elevation_deg * dtr);
double elevation_sigma_m = elevation_sigma_zenith_m / sin_el;
variance_m2 += elevation_sigma_m * elevation_sigma_m;
/*
* At or below the horizon the atmospheric corrections are being
* extrapolated rather than evaluated, and the geometry that produced
* them is not trustworthy either - a satellite reported below the
* horizon is usually arriving by reflection. Charge that explicitly
* instead of pretending the correction is as good as any other.
*/
if(isfinite(elevation_deg) && (elevation_deg <= 0.0))
{
variance_m2 += GPS_NAV_BELOW_HORIZON_SIGMA_M *
GPS_NAV_BELOW_HORIZON_SIGMA_M;
}
}
if(cn0_valid && isfinite(cn0_dbhz))
{
/*
* Receiver-specific code-tracking model. At 45 dB-Hz the additional
* sigma is 3 m; every 20 dB change scales amplitude-domain error by
* ten. Bounds prevent one channel dominating before calibration.
* Receiver-specific code-tracking model. DLL jitter goes as the
* inverse square root of carrier-to-noise density, so a 20 dB drop is
* 100 times less power and 10 times the sigma: at 45 dB-Hz the
* additional sigma is 3 m and it scales by ten every 20 dB. Bounds
* prevent one channel dominating before calibration.
*/
double cn0_sigma_m =
3.0 * pow(10.0, (45.0 - cn0_dbhz) / 20.0);
@@ -346,13 +407,28 @@ static double least_squares_measurement_weight(unsigned char ura_index,
{
cn0_sigma_m = 0.5;
}
if(cn0_sigma_m > 30.0)
if(cn0_sigma_m > GPS_NAV_CN0_SIGMA_MAX_M)
{
cn0_sigma_m = 30.0;
cn0_sigma_m = GPS_NAV_CN0_SIGMA_MAX_M;
}
variance_m2 += cn0_sigma_m * cn0_sigma_m;
}
return 1.0 / variance_m2;
return sqrt(variance_m2);
}
static double least_squares_measurement_weight(unsigned char ura_index,
double elevation_deg,
bool elevation_valid,
double cn0_dbhz,
bool cn0_valid)
{
double sigma_m = gps_nav_measurement_sigma_m(
ura_index, elevation_deg, elevation_valid, cn0_dbhz, cn0_valid);
if(!isfinite(sigma_m) || (sigma_m <= 0.0))
{
return 0.0;
}
return 1.0 / (sigma_m * sigma_m);
}
static bool least_squares_normalize_weights(double* weights, int count)
@@ -537,12 +613,15 @@ static double klobuchar_delay_meters(const ionoutc_t* ionoutc,
{
if((ionoutc == NULL) || !ionoutc->enable || !ionoutc->vflg ||
!isfinite(gps_tow) || !isfinite(latitude_deg) || !isfinite(longitude_deg) ||
!isfinite(azimuth_deg) || !isfinite(elevation_deg) || (elevation_deg <= 0.0))
!isfinite(azimuth_deg) || !isfinite(elevation_deg))
{
return 0.0;
}
double elevation_sc = elevation_deg / 180.0;
/* At or below the horizon the model is extrapolated rather than dropped;
* see least_squares_atmosphere_elevation_deg(). */
double elevation_sc =
least_squares_atmosphere_elevation_deg(elevation_deg) / 180.0;
double latitude_sc = latitude_deg / 180.0;
double longitude_sc = longitude_deg / 180.0;
double azimuth_rad = azimuth_deg * (PI / 180.0);
@@ -654,10 +733,12 @@ static bool least_squares_build_postfit(const mat_type* satpos,
geodetic.phi, geodetic.lambda,
topocentric.azim, topocentric.elv);
double trop = 0.0;
if((topocentric.elv > 0.0) &&
(topocentric.height >= -1000.0) && (topocentric.height <= 15000.0))
if((topocentric.height >= -1000.0) && (topocentric.height <= 15000.0))
{
trop = tropo(sin(topocentric.elv * dtr), topocentric.height / 1000.0);
trop = tropo(
sin(least_squares_atmosphere_elevation_deg(topocentric.elv) *
dtr),
topocentric.height / 1000.0);
if(!isfinite(trop) || (trop < 0.0))
{
trop = 0.0;
@@ -681,6 +762,18 @@ static bool least_squares_build_postfit(const mat_type* satpos,
static bool gps_wls_last_solution_valid = false;
static double gps_wls_last_solution[4] = {0.0, 0.0, 0.0, 0.0};
bool gps_nav_last_receiver_ecef(double ecef_out[3])
{
if((ecef_out == NULL) || !gps_wls_last_solution_valid)
{
return false;
}
ecef_out[0] = gps_wls_last_solution[0];
ecef_out[1] = gps_wls_last_solution[1];
ecef_out[2] = gps_wls_last_solution[2];
return true;
}
// Function leastSquaresSTD
// Definition : initialize all matc cpp class variables.
// Arguments :
@@ -921,11 +1014,15 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
i, iono, topcent->azim, topcent->elv);
}
trop = 0.0;
if((topcent->elv > 0.0) &&
(topcent->height >= -1000.0) && (topcent->height <= 15000.0))
if((topcent->height >= -1000.0) && (topcent->height <= 15000.0))
{
/* topocent height is metres; the atmospheric model uses kilometres. */
trop = tropo(sin(topcent->elv * dtr), topcent->height / 1000.0);
/* topocent height is metres; the atmospheric model uses
* kilometres. Elevation is clamped at the horizon rather
* than the correction being dropped there. */
trop = tropo(
sin(least_squares_atmosphere_elevation_deg(topcent->elv) *
dtr),
topcent->height / 1000.0);
if(!isfinite(trop) || (trop < 0.0))
{
trop = 0.0;
soc/software/gps_system/src/gps/gps_acq_manager.c
+103 -10
View File
@@ -8,18 +8,72 @@ static bool gps_acq_manager_prn_valid(unsigned int prn)
return prn < GPS_ACQ_MANAGER_PRN_COUNT;
}
static bool gps_acq_manager_prn_visible(
const gps_acq_manager_t* manager,
unsigned int prn)
{
return (manager->search_mask & (1u << prn)) != 0u;
}
static bool gps_acq_manager_retry_due(
const gps_acq_manager_t* manager,
const gps_acq_prn_entry_t* entry,
uint64_t now,
bool hot)
bool hot,
unsigned int prn)
{
uint64_t retry_delay = hot
? manager->config.hot_retry_delay_ticks
: manager->config.retry_delay_ticks;
return !entry->has_attempted ||
((now - entry->last_attempt_tick) >=
retry_delay);
/*
* A recently lost satellite is the best candidate there is: it was locked
* moments ago, so it is almost certainly still above the horizon. Serving
* it at the slow backoff from the very first retry inverted the priority -
* a never-seen cold PRN was retried every retry_delay_ticks while a
* known-good one waited hot_retry_delay_ticks, sixty times longer. Give
* the first few hot attempts the fast cadence and only escalate once the
* evidence has actually been contradicted.
*/
uint64_t retry_delay;
if(!hot)
{
retry_delay = manager->config.retry_delay_ticks;
}
else if(entry->hot_attempts < manager->config.hot_fast_attempts)
{
retry_delay = manager->config.retry_delay_ticks;
}
else
{
retry_delay = manager->config.hot_retry_delay_ticks;
}
/*
* A PRN the almanac places below the elevation mask cannot be acquired,
* and with a single acquisition engine every attempt spent on one is an
* attempt denied to a satellite that is actually up. Measured on the
* board, 75% of failed searches were against below-horizon PRNs. Slow
* them to a long cadence instead of dropping them: they must still be
* revisited so a wrong or stale visibility model self-corrects, and a
* satellite that has just risen is found without waiting for the next
* almanac refresh.
*/
if(!gps_acq_manager_prn_visible(manager, prn) &&
(manager->config.unlikely_retry_delay_ticks > retry_delay))
{
retry_delay = manager->config.unlikely_retry_delay_ticks;
}
/*
* has_attempted short-circuits the delay so a never-tried PRN is served
* immediately - but that must not let an invisible PRN jump the queue on
* the first sweep, which is exactly when the engine is busiest. Such a
* PRN still becomes due once the slow cadence has elapsed since boot, so
* it is delayed rather than excluded even if it is never attempted.
*/
if(!entry->has_attempted)
{
return gps_acq_manager_prn_visible(manager, prn) ||
(now >= manager->config.unlikely_retry_delay_ticks);
}
return (now - entry->last_attempt_tick) >= retry_delay;
}
static bool gps_acq_manager_hot_active(
@@ -51,7 +105,7 @@ static bool gps_acq_manager_find(
(now > entry->hot_until_tick)));
if(candidate &&
gps_acq_manager_retry_due(
manager, entry, now, hot))
manager, entry, now, hot, prn))
{
*prn_out = prn;
return true;
@@ -70,10 +124,16 @@ void gps_acq_manager_init(
}
memset(manager, 0, sizeof(*manager));
manager->config = *config;
/* Everything is searchable until a visibility model says otherwise. */
manager->search_mask = 0xffffffffu;
if(manager->config.max_hot_streak == 0u)
{
manager->config.max_hot_streak = 1u;
}
if(manager->config.hot_fast_attempts == 0u)
{
manager->config.hot_fast_attempts = 1u;
}
}
bool gps_acq_manager_enable_cold(
@@ -93,6 +153,29 @@ bool gps_acq_manager_enable_cold(
return entry->state == GPS_ACQ_PRN_COLD;
}
void gps_acq_manager_set_search_mask(
gps_acq_manager_t* manager,
uint32_t search_mask)
{
if(manager == NULL)
{
return;
}
/*
* An empty mask would stall acquisition entirely. Treat it as "no
* usable prediction" so a bad almanac degrades to the old blind sweep
* instead of to silence.
*/
manager->search_mask =
(search_mask == 0u) ? 0xffffffffu : search_mask;
}
uint32_t gps_acq_manager_search_mask(
const gps_acq_manager_t* manager)
{
return (manager != NULL) ? manager->search_mask : 0xffffffffu;
}
gps_acq_prn_state_t gps_acq_manager_state(
const gps_acq_manager_t* manager,
unsigned int prn)
@@ -150,6 +233,8 @@ void gps_acq_manager_mark_tracking(
manager->prn[prn].state = GPS_ACQ_PRN_TRACKING;
manager->prn[prn].attempt_was_hot = false;
manager->prn[prn].hot_until_tick = 0u;
/* Tracking succeeded, so the visibility evidence is fresh again. */
manager->prn[prn].hot_attempts = 0u;
}
void gps_acq_manager_mark_stale(
@@ -182,9 +267,11 @@ void gps_acq_manager_mark_lost(
entry->hot_until_tick = now + manager->config.hot_window_ticks;
if(!already_hot)
{
/* A real tracking loss gets one immediate reacquisition attempt.
* Later failures observe the hot retry backoff. */
/* A real tracking loss gets one immediate reacquisition attempt, and
* a fresh allowance of fast retries: the satellite was being tracked a
* moment ago, so it is the strongest candidate available. */
entry->has_attempted = false;
entry->hot_attempts = 0u;
}
}
@@ -200,6 +287,12 @@ void gps_acq_manager_mark_failed(
gps_acq_prn_entry_t* entry = &manager->prn[prn];
entry->last_attempt_tick = now;
entry->has_attempted = true;
if(entry->attempt_was_hot &&
(entry->hot_attempts < manager->config.hot_fast_attempts))
{
/* Only failures actually contradict the visibility evidence. */
entry->hot_attempts++;
}
entry->state =
entry->attempt_was_hot && (now <= entry->hot_until_tick)
? GPS_ACQ_PRN_HOT : GPS_ACQ_PRN_COLD;
soc/software/gps_system/src/gps/gps_almanac.c
+470
View File
@@ -0,0 +1,470 @@
#include "gps/gps_almanac.h"
#include <math.h>
#include <string.h>
#define GPS_ALMANAC_MU 3.986005e14 /* WGS-84 gravitational constant */
#define GPS_ALMANAC_OMEGA_E 7.2921151467e-5 /* WGS-84 earth rotation rate */
#define GPS_ALMANAC_PI 3.1415926535897932
#define GPS_ALMANAC_WEEK_SEC 604800.0
#define GPS_ALMANAC_HALF_WEEK_SEC 302400.0
/*
* Extract an LNAV field. Bit positions follow IS-GPS-200: bit 1 is the MSB
* of the 30-bit word and the low six bits carry parity, which is exactly the
* convention parse_subframe() already uses (for example URA at bits 13..16 of
* word 3 reads as (word >> 14) & 0xf).
*/
static uint32_t gps_almanac_field(unsigned int word,
unsigned int first_bit,
unsigned int length)
{
unsigned int shift = 30u - (first_bit + length - 1u);
uint32_t mask = (length >= 32u) ? 0xffffffffu : ((1u << length) - 1u);
return ((uint32_t)word >> shift) & mask;
}
static int32_t gps_almanac_signed(uint32_t value, unsigned int bits)
{
if((bits == 0u) || (bits >= 32u))
{
return (int32_t)value;
}
uint32_t sign = 1u << (bits - 1u);
return (int32_t)((value ^ sign) - sign);
}
static double gps_almanac_pow2(int exponent)
{
return ldexp(1.0, exponent);
}
void gps_almanac_reset(gps_almanac_t* almanac)
{
if(almanac == NULL)
{
return;
}
memset(almanac, 0, sizeof(*almanac));
}
bool gps_almanac_set_reference(gps_almanac_t* almanac,
unsigned int week,
double toa_sec)
{
/*
* One choke point for both the broadcast WNa and the flash restore, so a
* week that is arithmetically impossible cannot be adopted from either.
* Refusing it is strictly better than storing it: with no reference the
* assumed-week fallback still produces a usable prediction, whereas a
* nonsense week would put every satellite outside the propagation window
* and silently switch the visibility model off.
*/
if((almanac == NULL) || (week > GPS_ALMANAC_WEEK_LIMIT) ||
!isfinite(toa_sec) || (toa_sec < 0.0) ||
(toa_sec >= GPS_ALMANAC_WEEK_SEC))
{
return false;
}
almanac->week = week;
almanac->toa_sec = toa_sec;
almanac->reference_valid = true;
return true;
}
/*
* Almanac pages carry the transmitting-constellation SV ID in word 3. SV IDs
* 1..24 appear in subframe 5, 25..32 in subframe 4; every other SV ID on those
* subframes is a non-almanac page (ionosphere, UTC, health summaries, ...).
*/
static bool gps_almanac_page_is_almanac(unsigned char subframe_id,
unsigned char sv_id)
{
if((sv_id < 1u) || (sv_id > GPS_ALMANAC_PRN_COUNT))
{
return false;
}
if(subframe_id == 5u)
{
return sv_id <= 24u;
}
if(subframe_id == 4u)
{
return sv_id >= 25u;
}
return false;
}
bool gps_almanac_store_page(gps_almanac_t* almanac,
const unsigned int* subframe,
unsigned char subframe_id)
{
if((almanac == NULL) || (subframe == NULL))
{
return false;
}
unsigned char data_id =
(unsigned char)gps_almanac_field(subframe[2], 1u, 2u);
unsigned char sv_id =
(unsigned char)gps_almanac_field(subframe[2], 3u, 6u);
/* Data ID 1 is LNAV; anything else is a different message structure. */
if(data_id != 1u)
{
return false;
}
if(!gps_almanac_page_is_almanac(subframe_id, sv_id))
{
return false;
}
double ecc = (double)gps_almanac_field(subframe[2], 9u, 16u) *
gps_almanac_pow2(-21);
double toa = (double)gps_almanac_field(subframe[3], 1u, 8u) *
gps_almanac_pow2(12);
double delta_i =
(double)gps_almanac_signed(
gps_almanac_field(subframe[3], 9u, 16u), 16u) *
gps_almanac_pow2(-19);
double omega_dot =
(double)gps_almanac_signed(
gps_almanac_field(subframe[4], 1u, 16u), 16u) *
gps_almanac_pow2(-38);
unsigned char health =
(unsigned char)gps_almanac_field(subframe[4], 17u, 8u);
double sqrt_a = (double)gps_almanac_field(subframe[5], 1u, 24u) *
gps_almanac_pow2(-11);
double omega0 =
(double)gps_almanac_signed(
gps_almanac_field(subframe[6], 1u, 24u), 24u) *
gps_almanac_pow2(-23);
double omega =
(double)gps_almanac_signed(
gps_almanac_field(subframe[7], 1u, 24u), 24u) *
gps_almanac_pow2(-23);
double m0 =
(double)gps_almanac_signed(
gps_almanac_field(subframe[8], 1u, 24u), 24u) *
gps_almanac_pow2(-23);
/* af0 is split across word 10: bits 1..8 are the MSBs, bits 20..22 the LSBs. */
uint32_t af0_raw = (gps_almanac_field(subframe[9], 1u, 8u) << 3) |
gps_almanac_field(subframe[9], 20u, 3u);
double af0 = (double)gps_almanac_signed(af0_raw, 11u) *
gps_almanac_pow2(-20);
double af1 =
(double)gps_almanac_signed(
gps_almanac_field(subframe[9], 9u, 11u), 11u) *
gps_almanac_pow2(-38);
/*
* A dummy or not-yet-uploaded almanac page transmits zeros. A zero
* semi-major axis would divide by zero in the propagation, so reject the
* page rather than store an entry that cannot be evaluated.
*/
if((sqrt_a <= 1000.0) || (ecc < 0.0) || (ecc >= 1.0))
{
return false;
}
gps_almanac_entry_t* entry = &almanac->sv[sv_id - 1u];
entry->valid = true;
entry->prn = sv_id;
entry->health = health;
entry->ecc = ecc;
entry->toa_sec = toa;
entry->i0_rad =
(GPS_ALMANAC_I0_REF_SEMICIRCLES + delta_i) * GPS_ALMANAC_PI;
entry->omega_dot_rps = omega_dot * GPS_ALMANAC_PI;
entry->sqrt_a = sqrt_a;
entry->omega0_rad = omega0 * GPS_ALMANAC_PI;
entry->omega_rad = omega * GPS_ALMANAC_PI;
entry->m0_rad = m0 * GPS_ALMANAC_PI;
entry->af0_sec = af0;
entry->af1_sec_per_sec = af1;
return true;
}
unsigned int gps_almanac_valid_count(const gps_almanac_t* almanac)
{
if(almanac == NULL)
{
return 0u;
}
unsigned int count = 0u;
for(unsigned int i = 0u; i < GPS_ALMANAC_PRN_COUNT; i++)
{
if(almanac->sv[i].valid)
{
count++;
}
}
return count;
}
unsigned int gps_almanac_assumed_week(const gps_almanac_entry_t* entry,
unsigned int week,
double tow_sec)
{
if(entry == NULL)
{
return week;
}
/*
* Subframe 5 page 25 carries the almanac reference week, but it comes
* round only once per 25-frame cycle - up to 12.5 minutes. Waiting for it
* would leave the receiver sweeping blindly for exactly that window after
* every reset, which is when fast reacquisition matters most. Each
* almanac page carries its own toa, so the week is the only missing piece.
*
* The assumption made here, and the only one available, is that the
* almanac is less than half a week old. Under it the week is determined:
* pick the one that puts toa within half a week of now, which is the
* IS-GPS-200 20.3.3.5.2.3 crossover rule applied exactly once, at the one
* point where a week genuinely has to be guessed.
*
* Being advisory, the cost of guessing wrong is a PRN demoted to the slow
* cadence, never one that is unreachable - and the guess is replaced by
* the broadcast WNa, or by the persisted one, as soon as either exists.
*/
double raw = tow_sec - entry->toa_sec;
if(raw > GPS_ALMANAC_HALF_WEEK_SEC)
{
return week + 1u;
}
if((raw < -GPS_ALMANAC_HALF_WEEK_SEC) && (week > 0u))
{
return week - 1u;
}
return week;
}
bool gps_almanac_satellite_ecef(const gps_almanac_entry_t* entry,
unsigned int almanac_week,
unsigned int week,
double tow_sec,
double ecef_out[3])
{
if((entry == NULL) || !entry->valid || (ecef_out == NULL))
{
return false;
}
double a = entry->sqrt_a * entry->sqrt_a;
if(!isfinite(a) || (a <= 0.0))
{
return false;
}
/*
* tk is the true elapsed time since toa. The caller supplies the almanac's
* own week, so the week difference is real and must not be folded away.
*
* This used to wrap tk into the half week unconditionally, quoting the
* IS-GPS-200 20.3.3.5.2.3 rule. That rule is a *week-crossover* correction
* - it exists so a toa late in one week and a receiver time early in the
* next produce a small positive age - and applying it as a general
* reduction is wrong in two ways. It turns an old almanac into a
* confidently wrong one rather than an unusable one, because a GPS week is
* 14.04 orbital periods and not 14, so aliasing by one week displaces the
* satellite by about 14 degrees of orbital angle. And in loop form it ran
* once per week of error, so a week number that arrived corrupt turned one
* visibility refresh into billions of iterations: a hang, not a wrong
* answer. The crossover now belongs to the one caller that has to guess a
* week (gps_almanac_assumed_week); here the arithmetic is closed form and
* anything outside the usable window is simply refused.
*/
double tk = (tow_sec - entry->toa_sec) +
((double)week - (double)almanac_week) * GPS_ALMANAC_WEEK_SEC;
if(!isfinite(tk) || (fabs(tk) > GPS_ALMANAC_MAX_AGE_SEC))
{
return false;
}
double n0 = sqrt(GPS_ALMANAC_MU / (a * a * a));
double mk = entry->m0_rad + (n0 * tk);
/* Kepler, Newton-Raphson: converges in a handful of steps at almanac e. */
double ek = mk;
for(unsigned int iteration = 0u; iteration < 15u; iteration++)
{
double f = ek - (entry->ecc * sin(ek)) - mk;
double fp = 1.0 - (entry->ecc * cos(ek));
if(fabs(fp) < 1e-12)
{
break;
}
double delta = f / fp;
ek -= delta;
if(fabs(delta) < 1e-12)
{
break;
}
}
double sin_ek = sin(ek);
double cos_ek = cos(ek);
double nu = atan2(sqrt(1.0 - (entry->ecc * entry->ecc)) * sin_ek,
cos_ek - entry->ecc);
double phi = nu + entry->omega_rad;
double r = a * (1.0 - (entry->ecc * cos_ek));
double xp = r * cos(phi);
double yp = r * sin(phi);
double omega_k = entry->omega0_rad +
((entry->omega_dot_rps - GPS_ALMANAC_OMEGA_E) * tk) -
(GPS_ALMANAC_OMEGA_E * entry->toa_sec);
double cos_o = cos(omega_k);
double sin_o = sin(omega_k);
double cos_i = cos(entry->i0_rad);
double sin_i = sin(entry->i0_rad);
ecef_out[0] = (xp * cos_o) - (yp * cos_i * sin_o);
ecef_out[1] = (xp * sin_o) + (yp * cos_i * cos_o);
ecef_out[2] = yp * sin_i;
return isfinite(ecef_out[0]) && isfinite(ecef_out[1]) &&
isfinite(ecef_out[2]);
}
bool gps_almanac_look_angles(const gps_almanac_entry_t* entry,
unsigned int almanac_week,
unsigned int week,
double tow_sec,
const double receiver_ecef_m[3],
double* elevation_deg_out,
double* azimuth_deg_out)
{
if((receiver_ecef_m == NULL) || (elevation_deg_out == NULL))
{
return false;
}
double sat[3];
if(!gps_almanac_satellite_ecef(entry, almanac_week, week, tow_sec, sat))
{
return false;
}
double rx = receiver_ecef_m[0];
double ry = receiver_ecef_m[1];
double rz = receiver_ecef_m[2];
double ground = sqrt((rx * rx) + (ry * ry));
double radius = sqrt((rx * rx) + (ry * ry) + (rz * rz));
if(!isfinite(radius) || (radius < 1.0e6))
{
return false;
}
/*
* Geocentric local frame. The geodetic/geocentric latitude difference is
* up to ~0.19 degrees, which is irrelevant next to the elevation mask this
* feeds and avoids an iterative datum conversion in the ISR path.
*/
double sin_lat = rz / radius;
double cos_lat = ground / radius;
double sin_lon = (ground > 0.0) ? (ry / ground) : 0.0;
double cos_lon = (ground > 0.0) ? (rx / ground) : 1.0;
double dx = sat[0] - rx;
double dy = sat[1] - ry;
double dz = sat[2] - rz;
double up = (cos_lat * cos_lon * dx) + (cos_lat * sin_lon * dy) +
(sin_lat * dz);
double east = (-sin_lon * dx) + (cos_lon * dy);
double north = (-sin_lat * cos_lon * dx) + (-sin_lat * sin_lon * dy) +
(cos_lat * dz);
double horizontal = sqrt((east * east) + (north * north));
*elevation_deg_out = atan2(up, horizontal) * 180.0 / GPS_ALMANAC_PI;
if(azimuth_deg_out != NULL)
{
double azimuth = atan2(east, north) * 180.0 / GPS_ALMANAC_PI;
if(azimuth < 0.0)
{
azimuth += 360.0;
}
*azimuth_deg_out = azimuth;
}
return isfinite(*elevation_deg_out);
}
unsigned int gps_almanac_visible_mask(const gps_almanac_t* almanac,
unsigned int week,
double tow_sec,
const double receiver_ecef_m[3],
double mask_elevation_deg,
uint32_t* mask_out)
{
if(mask_out == NULL)
{
return 0u;
}
/*
* No almanac or no position: every PRN stays searchable. The scheduler
* must never be narrowed by a model it does not actually have.
*/
*mask_out = 0xffffffffu;
if((almanac == NULL) || (receiver_ecef_m == NULL))
{
return 0u;
}
uint32_t mask = 0u;
unsigned int visible = 0u;
unsigned int evaluated = 0u;
for(unsigned int i = 0u; i < GPS_ALMANAC_PRN_COUNT; i++)
{
const gps_almanac_entry_t* entry = &almanac->sv[i];
if(!entry->valid)
{
/*
* Unknown satellite: keep searching it, otherwise a partially
* collected almanac would hide PRNs we have simply not heard
* about yet.
*/
mask |= (1u << i);
continue;
}
evaluated++;
unsigned int almanac_week =
almanac->reference_valid
? almanac->week
: gps_almanac_assumed_week(entry, week, tow_sec);
double elevation_deg;
if(!gps_almanac_look_angles(entry, almanac_week, week, tow_sec,
receiver_ecef_m, &elevation_deg, NULL))
{
/*
* Too old, or otherwise not propagable. This is deliberately
* ahead of the health test: an almanac that cannot say where a
* satellite is cannot be trusted to say whether it is well
* either, and health only ever demotes. Since persistence, an
* entry can be weeks old and still carry a health byte that was
* true at the time and is not now.
*/
mask |= (1u << i);
continue;
}
if(entry->health != 0u)
{
continue;
}
if(elevation_deg >= mask_elevation_deg)
{
mask |= (1u << i);
visible++;
}
}
if(evaluated == 0u)
{
*mask_out = 0xffffffffu;
return 0u;
}
*mask_out = mask;
return visible;
}
soc/software/gps_system/src/gps/gps_ephemeris_cache.c
+128
View File
@@ -3,6 +3,8 @@
#include <stddef.h>
#include <string.h>
#include "gps/gps_ephemeris_validity.h"
void gps_ephemeris_cache_init(
gps_ephemeris_cache_t* cache,
uint64_t max_age_ticks)
@@ -66,3 +68,129 @@ void gps_ephemeris_cache_invalidate(
}
memset(&cache->entry[prn], 0, sizeof(cache->entry[prn]));
}
bool gps_ephemeris_cache_store_if_new(
gps_ephemeris_cache_t* cache,
unsigned int prn,
const gps_ephemeris_t* ephemeris,
uint64_t now,
bool* changed_out)
{
if(changed_out != NULL)
{
*changed_out = false;
}
if((cache == NULL) || (ephemeris == NULL) || (prn >= GPS_PRN_COUNT) ||
(gps_ephemeris_check_static(ephemeris, prn) != GPS_EPHEMERIS_OK))
{
return false;
}
gps_ephemeris_cache_entry_t* entry = &cache->entry[prn];
const gps_ephemeris_t* current = entry->valid ?
&entry->ephemeris : NULL;
/*
* Anything heard on the air outranks a record read back from flash. The
* Toe ordering cannot make that call: Toe is a second-of-week, so once the
* persisted record is more than half a week old the signed half-week rule
* reads the live broadcast as the *older* of the two and refuses it, which
* would pin the cache and the flash to the stale copy indefinitely.
*/
if((current != NULL) && entry->restored)
{
/* current aliases entry->ephemeris, so decide before overwriting it. */
bool same = gps_ephemeris_same_issue(ephemeris, current);
entry->ephemeris = *ephemeris;
entry->committed_tick = now;
entry->restored = false;
if(changed_out != NULL)
{
*changed_out = !same;
}
return true;
}
if(!gps_ephemeris_supersedes(ephemeris, current))
{
/* Same data set: refresh the age datum only, so a satellite that
* keeps rebroadcasting one ephemeris does not expire while it is
* still being received. */
if((current != NULL) &&
gps_ephemeris_same_issue(ephemeris, current))
{
entry->committed_tick = now;
return true;
}
return false;
}
entry->ephemeris = *ephemeris;
entry->committed_tick = now;
entry->valid = true;
if(changed_out != NULL)
{
*changed_out = true;
}
return true;
}
bool gps_ephemeris_cache_store_restored(
gps_ephemeris_cache_t* cache,
unsigned int prn,
const gps_ephemeris_t* ephemeris,
uint64_t now)
{
if((cache == NULL) || (ephemeris == NULL) || (prn >= GPS_PRN_COUNT) ||
(gps_ephemeris_check_static(ephemeris, prn) != GPS_EPHEMERIS_OK))
{
return false;
}
gps_ephemeris_cache_entry_t* entry = &cache->entry[prn];
/*
* Never displace something already learned this run. Flash is read once at
* boot, but ordering between that read and the first decoded subframe is
* not guaranteed, and the live record is always the better one.
*/
if(entry->valid && !entry->restored)
{
return false;
}
entry->ephemeris = *ephemeris;
entry->committed_tick = now;
entry->valid = true;
entry->restored = true;
return true;
}
bool gps_ephemeris_cache_restore_at(
gps_ephemeris_cache_t* cache,
unsigned int prn,
uint64_t now,
bool time_known,
int week,
double tow_sec,
gps_ephemeris_t* ephemeris_out)
{
gps_ephemeris_t candidate;
if(!gps_ephemeris_cache_restore(cache, prn, now, &candidate))
{
return false;
}
gps_ephemeris_status_t status = gps_ephemeris_check(
&candidate, prn, time_known, week, tow_sec);
if(status != GPS_EPHEMERIS_OK)
{
/* An age rejection is permanent for this record; a structural
* rejection means it should never have been cached. Either way the
* slot is cleared so the receiver falls back to broadcast decode. */
gps_ephemeris_cache_invalidate(cache, prn);
return false;
}
if(ephemeris_out != NULL)
{
*ephemeris_out = candidate;
}
return true;
}
soc/software/gps_system/src/gps/gps_ephemeris_store.c
+529
View File
@@ -0,0 +1,529 @@
#include "gps/gps_ephemeris_store.h"
#include <stddef.h>
#include <string.h>
#include "gps/gps_ephemeris_validity.h"
/*
* On-flash record. Explicitly sized and laid out; nothing here depends on
* compiler struct packing beyond the fixed-width members, and the whole body is
* covered by the trailing CRC. Anything not written is left at the erased
* value, so a torn write cannot look valid.
*/
typedef struct {
uint32_t magic;
uint16_t version;
uint16_t record_len;
uint32_t sequence;
uint32_t toe_sec;
uint16_t week;
uint16_t iodc;
uint8_t prn;
uint8_t iode;
uint8_t health;
uint8_t flags;
uint32_t words[30];
uint32_t crc32;
} gps_eph_record_t;
typedef struct {
uint32_t magic;
uint16_t version;
uint16_t reserved;
uint32_t generation;
uint32_t crc32;
} gps_eph_sector_header_t;
_Static_assert(sizeof(gps_eph_record_t) <= GPS_EPH_STORE_SLOT_SIZE,
"ephemeris record does not fit one flash page");
_Static_assert(sizeof(gps_eph_sector_header_t) <= GPS_EPH_STORE_SLOT_SIZE,
"ephemeris store header does not fit one flash page");
uint32_t gps_ephemeris_store_crc32(const void* data, uint32_t len)
{
const uint8_t* bytes = (const uint8_t*)data;
uint32_t crc = 0xffffffffu;
for(uint32_t i = 0u; i < len; i++)
{
crc ^= bytes[i];
for(unsigned int bit = 0u; bit < 8u; bit++)
{
uint32_t mask = (uint32_t)(-(int32_t)(crc & 1u));
crc = (crc >> 1) ^ (0xedb88320u & mask);
}
}
return ~crc;
}
static uint32_t store_sector_address(const gps_ephemeris_store_t* store,
unsigned int sector)
{
return store->flash.base_address +
((uint32_t)sector * store->flash.sector_size);
}
static uint32_t store_slot_address(const gps_ephemeris_store_t* store,
unsigned int sector,
uint32_t slot)
{
return store_sector_address(store, sector) +
(slot * GPS_EPH_STORE_SLOT_SIZE);
}
static bool record_valid(const gps_eph_record_t* record)
{
if((record->magic != GPS_EPH_STORE_MAGIC) ||
(record->version != GPS_EPH_STORE_VERSION) ||
(record->record_len != (uint16_t)sizeof(*record)) ||
(record->prn >= GPS_PRN_COUNT))
{
return false;
}
uint32_t crc = gps_ephemeris_store_crc32(
record, (uint32_t)offsetof(gps_eph_record_t, crc32));
return crc == record->crc32;
}
static bool header_valid(const gps_eph_sector_header_t* header)
{
if((header->magic != GPS_EPH_STORE_HEADER_MAGIC) ||
(header->version != GPS_EPH_STORE_VERSION))
{
return false;
}
uint32_t crc = gps_ephemeris_store_crc32(
header, (uint32_t)offsetof(gps_eph_sector_header_t, crc32));
return crc == header->crc32;
}
/*
* A record is newer than another for the same PRN when its sequence is newer in
* modular arithmetic. Sequence is assigned by this module and only ever
* increments, so a simple signed difference is unambiguous.
*/
static bool sequence_newer(uint32_t candidate, uint32_t current)
{
return (int32_t)(candidate - current) > 0;
}
static void store_reset_index(gps_ephemeris_store_t* store)
{
memset(store->prn_sequence, 0, sizeof(store->prn_sequence));
memset(store->prn_present, 0, sizeof(store->prn_present));
store->sequence = 0u;
store->records_scanned = 0u;
store->records_rejected = 0u;
}
/* Walk one sector's slots, updating the per-PRN index and the fill level. */
static void store_scan_sector(gps_ephemeris_store_t* store,
unsigned int sector,
uint32_t* next_slot_out)
{
uint32_t next_slot = 1u; /* slot 0 is the header */
for(uint32_t slot = 1u; slot < store->slots_per_sector; slot++)
{
gps_eph_record_t record;
store->flash.read(store->flash.ctx,
store_slot_address(store, sector, slot),
&record, (uint32_t)sizeof(record));
if(record.magic != GPS_EPH_STORE_MAGIC)
{
/* First never-written slot ends the log. */
break;
}
next_slot = slot + 1u;
store->records_scanned++;
if(!record_valid(&record))
{
/* Torn or corrupted: skip it but keep walking, because a later
* slot may hold a good record for the same PRN. */
store->records_rejected++;
continue;
}
unsigned int prn = record.prn;
if(!store->prn_present[prn] ||
sequence_newer(record.sequence, store->prn_sequence[prn]))
{
store->prn_present[prn] = true;
store->prn_sequence[prn] = record.sequence;
}
if(sequence_newer(record.sequence, store->sequence))
{
store->sequence = record.sequence;
}
}
*next_slot_out = next_slot;
}
bool gps_ephemeris_store_mount(
gps_ephemeris_store_t* store,
const gps_ephemeris_store_flash_t* flash)
{
if((store == NULL) || (flash == NULL) ||
(flash->read == NULL) || (flash->erase_sector == NULL) ||
(flash->program == NULL) ||
(flash->sector_size < (2u * GPS_EPH_STORE_SLOT_SIZE)) ||
((flash->sector_size % GPS_EPH_STORE_SLOT_SIZE) != 0u))
{
return false;
}
memset(store, 0, sizeof(*store));
store->flash = *flash;
store->slots_per_sector = flash->sector_size / GPS_EPH_STORE_SLOT_SIZE;
store_reset_index(store);
gps_eph_sector_header_t header[2];
bool valid[2];
for(unsigned int sector = 0u; sector < 2u; sector++)
{
store->flash.read(store->flash.ctx,
store_sector_address(store, sector),
&header[sector], (uint32_t)sizeof(header[sector]));
valid[sector] = header_valid(&header[sector]);
}
if(!valid[0] && !valid[1])
{
/* Nothing usable on flash. Mount empty rather than erasing: the
* caller decides when it is safe to spend an erase cycle. */
store->mounted = true;
store->active_sector = 0u;
store->generation = 0u;
store->next_slot = store->slots_per_sector; /* forces a format */
return true;
}
unsigned int active;
if(valid[0] && valid[1])
{
/* Both headers survived: the newer generation is the live one. This
* is the state left by a power loss during compaction. */
active = sequence_newer(header[1].generation, header[0].generation) ?
1u : 0u;
}
else
{
active = valid[0] ? 0u : 1u;
}
store->active_sector = active;
store->generation = header[active].generation;
store_scan_sector(store, active, &store->next_slot);
store->mounted = true;
return true;
}
void gps_ephemeris_store_limit_slots(
gps_ephemeris_store_t* store,
uint32_t max_slots)
{
if((store == NULL) || !store->mounted || (max_slots < 2u))
{
return;
}
if(max_slots < store->slots_per_sector)
{
store->slots_per_sector = max_slots;
if(store->next_slot > max_slots)
{
store->next_slot = max_slots;
}
}
}
static bool store_write_header(gps_ephemeris_store_t* store,
unsigned int sector,
uint32_t generation)
{
gps_eph_sector_header_t header;
memset(&header, 0, sizeof(header));
header.magic = GPS_EPH_STORE_HEADER_MAGIC;
header.version = GPS_EPH_STORE_VERSION;
header.generation = generation;
header.crc32 = gps_ephemeris_store_crc32(
&header, (uint32_t)offsetof(gps_eph_sector_header_t, crc32));
return store->flash.program(store->flash.ctx,
store_sector_address(store, sector),
&header, (uint32_t)sizeof(header));
}
bool gps_ephemeris_store_format(gps_ephemeris_store_t* store)
{
if((store == NULL) || !store->mounted)
{
return false;
}
for(unsigned int sector = 0u; sector < 2u; sector++)
{
if(!store->flash.erase_sector(store->flash.ctx,
store_sector_address(store, sector)))
{
return false;
}
store->erases++;
}
if(!store_write_header(store, 0u, 1u))
{
return false;
}
store->active_sector = 0u;
store->generation = 1u;
store->next_slot = 1u;
store_reset_index(store);
return true;
}
static bool store_append_record(gps_ephemeris_store_t* store,
const gps_eph_record_t* record)
{
if(store->next_slot >= store->slots_per_sector)
{
return false;
}
uint32_t address =
store_slot_address(store, store->active_sector, store->next_slot);
if(!store->flash.program(store->flash.ctx, address, record,
(uint32_t)sizeof(*record)))
{
return false;
}
store->next_slot++;
store->writes++;
return true;
}
/*
* Copy the newest record of every PRN into the spare sector at generation+1,
* then erase the old one. Ordering matters: the new sector is complete and
* carries the higher generation before anything is destroyed, so a power loss
* at any point leaves a mountable store.
*/
static bool store_compact(gps_ephemeris_store_t* store)
{
unsigned int spare = (store->active_sector == 0u) ? 1u : 0u;
unsigned int source = store->active_sector;
uint32_t source_slots = store->next_slot;
if(!store->flash.erase_sector(store->flash.ctx,
store_sector_address(store, spare)))
{
return false;
}
store->erases++;
if(!store_write_header(store, spare, store->generation + 1u))
{
return false;
}
uint32_t written = 1u;
for(unsigned int prn = 0u; prn < GPS_PRN_COUNT; prn++)
{
if(!store->prn_present[prn])
{
continue;
}
/* Find the record carrying this PRN's winning sequence. */
for(uint32_t slot = source_slots; slot > 1u; slot--)
{
gps_eph_record_t record;
store->flash.read(store->flash.ctx,
store_slot_address(store, source, slot - 1u),
&record, (uint32_t)sizeof(record));
if(!record_valid(&record) || (record.prn != prn) ||
(record.sequence != store->prn_sequence[prn]))
{
continue;
}
if(written >= store->slots_per_sector)
{
/* The live set does not fit. Latch it: the spare sector is
* already erased, so retrying would erase twice per save
* forever. */
store->capacity_exceeded = true;
return false;
}
if(!store->flash.program(
store->flash.ctx,
store_slot_address(store, spare, written),
&record, (uint32_t)sizeof(record)))
{
return false;
}
written++;
store->writes++;
break;
}
}
if(!store->flash.erase_sector(store->flash.ctx,
store_sector_address(store, source)))
{
return false;
}
store->erases++;
store->active_sector = spare;
store->generation++;
store->next_slot = written;
store->compactions++;
return true;
}
bool gps_ephemeris_store_save(
gps_ephemeris_store_t* store,
unsigned int prn,
const gps_ephemeris_t* ephemeris,
const unsigned int subframe_words[3][10],
bool* wrote_out)
{
if(wrote_out != NULL)
{
*wrote_out = false;
}
if((store == NULL) || !store->mounted || store->capacity_exceeded ||
(ephemeris == NULL) ||
(subframe_words == NULL) || (prn >= GPS_PRN_COUNT) ||
(gps_ephemeris_check_static(ephemeris, prn) != GPS_EPHEMERIS_OK))
{
return false;
}
if(store->next_slot >= store->slots_per_sector)
{
/* Either never formatted, or the active sector is full. */
if(store->generation == 0u)
{
if(!gps_ephemeris_store_format(store))
{
return false;
}
}
else if(!store_compact(store))
{
return false;
}
/*
* Compaction can succeed and still leave no free slot, when the live
* set exactly fills the sector. The append below would then fail and
* the next save would compact again, erasing two more sectors every
* time. Latch instead: this is a configuration that cannot hold the
* live set, and wearing the flash out is worse than losing the cache.
*/
if(store->next_slot >= store->slots_per_sector)
{
store->capacity_exceeded = true;
return false;
}
}
gps_eph_record_t record;
memset(&record, 0, sizeof(record));
record.magic = GPS_EPH_STORE_MAGIC;
record.version = GPS_EPH_STORE_VERSION;
record.record_len = (uint16_t)sizeof(record);
record.sequence = store->sequence + 1u;
record.toe_sec = (uint32_t)ephemeris->toe.sec;
record.week = (uint16_t)ephemeris->WN;
record.iodc = (uint16_t)ephemeris->iodc;
record.prn = (uint8_t)prn;
record.iode = ephemeris->iode;
record.health = ephemeris->svhlth;
record.flags = ephemeris->fit_flag ? 1u : 0u;
for(unsigned int sf = 0u; sf < 3u; sf++)
{
for(unsigned int word = 0u; word < 10u; word++)
{
record.words[(sf * 10u) + word] =
(uint32_t)subframe_words[sf][word];
}
}
record.crc32 = gps_ephemeris_store_crc32(
&record, (uint32_t)offsetof(gps_eph_record_t, crc32));
if(!store_append_record(store, &record))
{
return false;
}
store->sequence = record.sequence;
store->prn_present[prn] = true;
store->prn_sequence[prn] = record.sequence;
if(wrote_out != NULL)
{
*wrote_out = true;
}
return true;
}
unsigned int gps_ephemeris_store_load_all(
gps_ephemeris_store_t* store,
gps_ephemeris_store_load_fn handler,
void* ctx)
{
if((store == NULL) || !store->mounted || (handler == NULL))
{
return 0u;
}
unsigned int delivered = 0u;
for(unsigned int prn = 0u; prn < GPS_PRN_COUNT; prn++)
{
if(!store->prn_present[prn])
{
continue;
}
for(uint32_t slot = store->next_slot; slot > 1u; slot--)
{
gps_eph_record_t record;
store->flash.read(
store->flash.ctx,
store_slot_address(store, store->active_sector, slot - 1u),
&record, (uint32_t)sizeof(record));
if(!record_valid(&record) || (record.prn != prn) ||
(record.sequence != store->prn_sequence[prn]))
{
continue;
}
/* Rebuild through the live decoder rather than trusting a
* serialised structure. */
gps_frame_t frame;
memset(&frame, 0, sizeof(frame));
frame.gps_ephems.SVID = (unsigned char)prn;
bool parsed = true;
for(unsigned int sf = 0u; sf < 3u; sf++)
{
unsigned int words[10];
for(unsigned int word = 0u; word < 10u; word++)
{
words[word] = record.words[(sf * 10u) + word];
}
unsigned int how_tow = 0u;
unsigned char id = parse_subframe(words, &frame, &how_tow);
if(id != (unsigned char)(sf + 1u))
{
parsed = false;
break;
}
}
if(!parsed || !frame.gps_ephems.ephemeris_valid)
{
store->records_rejected++;
break;
}
frame.gps_ephems.SVID = (unsigned char)prn;
if(gps_ephemeris_check_static(&frame.gps_ephems, prn) !=
GPS_EPHEMERIS_OK)
{
store->records_rejected++;
break;
}
handler(ctx, prn, &frame.gps_ephems);
delivered++;
break;
}
}
return delivered;
}
soc/software/gps_system/src/gps/gps_ephemeris_validity.c
+231
View File
@@ -0,0 +1,231 @@
#include "gps/gps_ephemeris_validity.h"
#include <math.h>
#include <stddef.h>
#include "gps/gps_time_anchor.h"
double gps_ephemeris_fit_interval_hours(unsigned int iodc, bool fit_flag)
{
if(!fit_flag)
{
return GPS_EPHEMERIS_NOMINAL_FIT_HOURS;
}
/* IS-GPS-200 Table 20-XII, the ranges the ICD tabulates for a set fit
* interval flag. IODC is ten bits, so nothing above 1023 can occur. */
if((iodc >= 240u) && (iodc <= 247u))
{
return 8.0;
}
if(((iodc >= 248u) && (iodc <= 255u)) || (iodc == 496u))
{
return 14.0;
}
if((iodc >= 497u) && (iodc <= 503u))
{
return 26.0;
}
if((iodc >= 504u) && (iodc <= 510u))
{
return 50.0;
}
if(iodc == 511u)
{
return 74.0;
}
return GPS_EPHEMERIS_LONG_FIT_FALLBACK_HOURS;
}
double gps_ephemeris_max_toe_offset_sec(const gps_ephemeris_t* eph)
{
if(eph == NULL)
{
return 0.0;
}
return (gps_ephemeris_fit_interval_hours(eph->iodc, eph->fit_flag) *
3600.0) / 2.0;
}
bool gps_ephemeris_toe_offset_sec(
const gps_ephemeris_t* eph,
int week,
double tow_sec,
double* offset_sec_out)
{
if((eph == NULL) || (offset_sec_out == NULL) ||
!isfinite(tow_sec) || !isfinite(eph->toe.sec) ||
(tow_sec < 0.0) || (tow_sec >= GPS_TIME_WEEK_SECONDS) ||
(eph->toe.sec < 0.0) || (eph->toe.sec >= GPS_TIME_WEEK_SECONDS) ||
(week < 0))
{
return false;
}
/*
* Toe carries a week resolved at commit time from the subframe 1 week
* number and the handover word (gps_resolve_toe_week()), so the age is the
* true elapsed time and needs no reduction.
*
* The signed half-week rule that used to stand in for the week is not
* good enough: with only two second-of-week values to compare, a record
* exactly one week old is indistinguishable from one broadcast seconds
* ago. That is unreachable while ephemeris lives in RAM and expires in
* hours, and directly reachable once it is persisted across a power-off -
* a receiver switched on a week later at a similar time of day would
* accept its own week-old orbits as current.
*
* A record whose week could not be resolved gets no age, which reads as a
* rejection at every caller. That is the safe direction: it forces a
* fresh decode rather than trusting an orbit of unknown vintage.
*/
if(!eph->toe_week_valid)
{
return false;
}
*offset_sec_out = (((double)week - (double)eph->toe.week) *
GPS_TIME_WEEK_SECONDS) + (tow_sec - eph->toe.sec);
return true;
}
const char* gps_ephemeris_status_name(gps_ephemeris_status_t status)
{
switch(status)
{
case GPS_EPHEMERIS_OK: return "ok";
case GPS_EPHEMERIS_REJECT_NULL: return "null";
case GPS_EPHEMERIS_REJECT_UNCOMMITTED: return "uncommitted";
case GPS_EPHEMERIS_REJECT_PRN: return "prn";
case GPS_EPHEMERIS_REJECT_IOD: return "iod";
case GPS_EPHEMERIS_REJECT_HEALTH: return "health";
case GPS_EPHEMERIS_REJECT_ACCURACY: return "accuracy";
case GPS_EPHEMERIS_REJECT_PARAMETERS: return "parameters";
case GPS_EPHEMERIS_REJECT_AGE: return "age";
default: return "unknown";
}
}
gps_ephemeris_status_t gps_ephemeris_check_static(
const gps_ephemeris_t* eph,
unsigned int expected_prn)
{
if(eph == NULL)
{
return GPS_EPHEMERIS_REJECT_NULL;
}
if(!eph->ephemeris_valid)
{
return GPS_EPHEMERIS_REJECT_UNCOMMITTED;
}
if((expected_prn >= GPS_PRN_COUNT) || (eph->SVID != expected_prn))
{
return GPS_EPHEMERIS_REJECT_PRN;
}
/* IS-GPS-200: the eight IODE bits equal the eight LSBs of the IODC of
* the same data set. A mismatch means the record was assembled from
* two different uploads. */
if((unsigned int)eph->iode != (eph->iodc & 0xffu))
{
return GPS_EPHEMERIS_REJECT_IOD;
}
if((eph->svhlth != 0u) || (eph->update != 0u))
{
return GPS_EPHEMERIS_REJECT_HEALTH;
}
/* URA index 15 means "no accuracy prediction"; the ICD tells the user
* not to use the satellite. */
if(eph->sva >= 15u)
{
return GPS_EPHEMERIS_REJECT_ACCURACY;
}
if(!isfinite(eph->A) || !(eph->A > 0.0) ||
!isfinite(eph->ecc) || (eph->ecc < 0.0) || (eph->ecc >= 1.0) ||
!isfinite(eph->sqrta) || !(eph->sqrta > 0.0) ||
!isfinite(eph->toe.sec) ||
(eph->toe.sec < 0.0) || (eph->toe.sec >= GPS_TIME_WEEK_SECONDS) ||
!isfinite(eph->toc.sec) ||
(eph->toc.sec < 0.0) || (eph->toc.sec >= GPS_TIME_WEEK_SECONDS))
{
return GPS_EPHEMERIS_REJECT_PARAMETERS;
}
return GPS_EPHEMERIS_OK;
}
gps_ephemeris_status_t gps_ephemeris_check(
const gps_ephemeris_t* eph,
unsigned int expected_prn,
bool time_known,
int week,
double tow_sec)
{
gps_ephemeris_status_t status =
gps_ephemeris_check_static(eph, expected_prn);
if((status != GPS_EPHEMERIS_OK) || !time_known)
{
return status;
}
double offset_sec = 0.0;
if(!gps_ephemeris_toe_offset_sec(eph, week, tow_sec, &offset_sec))
{
return GPS_EPHEMERIS_REJECT_AGE;
}
if(fabs(offset_sec) > gps_ephemeris_max_toe_offset_sec(eph))
{
return GPS_EPHEMERIS_REJECT_AGE;
}
return GPS_EPHEMERIS_OK;
}
bool gps_ephemeris_same_issue(
const gps_ephemeris_t* first,
const gps_ephemeris_t* second)
{
if((first == NULL) || (second == NULL))
{
return false;
}
return (first->SVID == second->SVID) &&
(first->iode == second->iode) &&
(first->iodc == second->iodc) &&
(first->toe.sec == second->toe.sec) &&
(first->toc.sec == second->toc.sec);
}
bool gps_ephemeris_supersedes(
const gps_ephemeris_t* candidate,
const gps_ephemeris_t* current)
{
if((candidate == NULL) || !candidate->ephemeris_valid)
{
return false;
}
if((current == NULL) || !current->ephemeris_valid)
{
return true;
}
if(candidate->SVID != current->SVID)
{
return false;
}
if(gps_ephemeris_same_issue(candidate, current))
{
/* Same data set decoded again. Health or the transmitted URA can
* still change within one issue of data; accept those, reject the
* pure repeat so no flash write is generated. */
return (candidate->svhlth != current->svhlth) ||
(candidate->sva != current->sva);
}
/* Different issue of data. Accept only when Toe advances, using the
* signed half-week rule so a week rollover is not read as a jump back. */
double delta = candidate->toe.sec - current->toe.sec;
if(delta > GPS_TIME_HALF_WEEK_SECONDS)
{
delta -= GPS_TIME_WEEK_SECONDS;
}
else if(delta < -GPS_TIME_HALF_WEEK_SECONDS)
{
delta += GPS_TIME_WEEK_SECONDS;
}
return delta > 0.0;
}
soc/software/gps_system/src/gps/gps_interrupt_handlers.c
+1214 -21
View File
File diff suppressed because it is too large Load Diff
soc/software/gps_system/src/gps/gps_ip.c
+111 -389
View File
@@ -5,6 +5,7 @@
#include <string.h>
#include "gps/NavEquations.h"
#include "gps/Navigation.h"
#include "gps/gps_ephemeris_validity.h"
#include "gps/gps_ip_defines.h"
#include "gps/linked_list.h"
#include "gps/gps_interrupt_handlers.h"
@@ -60,8 +61,6 @@ volatile gps_frame_t gps_frames_parsed[GPS_TRCK_IP_COUNT];
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;
@@ -127,28 +126,6 @@ uint64_t gps_get_systick(void)
return gps_systick_ticks;
}
bool gps_get_ionoutc(ionoutc_t* ionoutc)
{
if((ionoutc == NULL) || !gps_ionoutc_valid)
{
return false;
}
*ionoutc = gps_last_ionoutc;
return true;
}
static double gps_unwrap_corrected_abs_sample(uint32_t raw_sample,
double corrected_sample,
uint32_t reference_sample)
{
/* All accepted samples are within the coherence window, so their signed
* modulo-2^32 distance from the newest sample is unambiguous. */
int32_t delta = (int32_t)(raw_sample - reference_sample);
double code_phase_correction = (double)raw_sample - corrected_sample;
return (double)reference_sample + (double)delta - code_phase_correction;
}
static bool gps_snapshot_has_svid(const gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
unsigned int count,
unsigned char svid)
@@ -165,354 +142,6 @@ static bool gps_snapshot_has_svid(const gps_ephemeris_t snapshot[GPS_TRCK_IP_COU
static uint32_t gps_nav_epoch_reject_total;
static int32_t gps_sign_extend(uint32_t value, unsigned int bits)
{
if((bits == 0u) || (bits >= 32u))
{
return (int32_t)value;
}
uint32_t sign = 1u << (bits - 1u);
return (int32_t)((value ^ sign) - sign);
}
static unsigned char gps_sf4_page_from_sv_id(unsigned char sv_id)
{
if((sv_id >= 25u) && (sv_id <= 28u))
{
return (unsigned char)(sv_id - 23u); /* pages 2..5 */
}
if((sv_id >= 29u) && (sv_id <= 32u))
{
return (unsigned char)(sv_id - 22u); /* pages 7..10 */
}
switch(sv_id)
{
case 52u: return 13u;
case 53u: return 14u;
case 54u: return 15u;
case 55u: return 17u;
case 56u: return 18u;
case 58u: return 19u;
case 59u: return 20u;
case 60u: return 22u;
case 61u: return 23u;
case 63u: return 25u;
default: return 0u;
}
}
static unsigned char gps_sf5_page_from_sv_id(unsigned char sv_id)
{
if((sv_id >= 1u) && (sv_id <= 24u))
{
return sv_id;
}
return (sv_id == 51u) ? 25u : 0u;
}
static void gps_parse_subframe4_page18(const unsigned int* subframe, gps_ephemeris_t* eph)
{
ionoutc_t* ion = &eph->ionoutc;
uint32_t word3 = subframe[2];
uint32_t word4 = subframe[3];
uint32_t word5 = subframe[4];
uint32_t word6 = subframe[5];
uint32_t word7 = subframe[6];
uint32_t word8 = subframe[7];
uint32_t word9 = subframe[8];
uint32_t word10 = subframe[9];
/* The IP has already undone LNAV D30* data inversion. These casts only
* interpret the extracted ICD signed fields at their specified width. */
ion->alpha0 = (double)(int8_t)((word3 >> 14) & 0xffu) * P2_30;
ion->alpha1 = (double)(int8_t)((word3 >> 6) & 0xffu) * P2_27;
ion->alpha2 = (double)(int8_t)((word4 >> 22) & 0xffu) * P2_24;
ion->alpha3 = (double)(int8_t)((word4 >> 14) & 0xffu) * P2_24;
ion->beta0 = (double)(int8_t)((word4 >> 6) & 0xffu) * 2048.0;
ion->beta1 = (double)(int8_t)((word5 >> 22) & 0xffu) * 16384.0;
ion->beta2 = (double)(int8_t)((word5 >> 14) & 0xffu) * 65536.0;
ion->beta3 = (double)(int8_t)((word5 >> 6) & 0xffu) * 65536.0;
ion->A1 = (double)gps_sign_extend((word6 >> 6) & 0xffffffu, 24u) * P2_50;
uint32_t a0_raw = (((word7 >> 6) & 0xffffffu) << 8) | ((word8 >> 22) & 0xffu);
int64_t a0_signed = (a0_raw & 0x80000000u) != 0u
? (int64_t)a0_raw - INT64_C(4294967296)
: (int64_t)a0_raw;
ion->A0 = (double)a0_signed * P2_30;
ion->tot = (int)(((word8 >> 14) & 0xffu) << 12);
ion->wnt = (int)((word8 >> 6) & 0xffu);
ion->dtls = (int)(int8_t)((word9 >> 22) & 0xffu);
ion->wnlsf = (int)((word9 >> 14) & 0xffu);
ion->dn = (int)((word9 >> 6) & 0xffu);
ion->dtlsf = (int)(int8_t)((word10 >> 22) & 0xffu);
ion->enable = true;
ion->vflg = true;
gps_last_ionoutc = *ion;
gps_ionoutc_valid = true;
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
"subframe 4 page 18 iono parsed: a=[%.3e %.3e %.3e %.3e] b=[%.3e %.3e %.3e %.3e]\r\n",
ion->alpha0, ion->alpha1, ion->alpha2, ion->alpha3,
ion->beta0, ion->beta1, ion->beta2, ion->beta3);
}
void acq_afe_start(volatile gps_acq_t* gps_handle)
{
gps_handle->CONTROL_REG |= (1 << GPS_ACQ_AFE_START);
}
void acq_enable_intr(volatile gps_acq_t* gps_handle, unsigned char intr_no)
{
(void)intr_no;
// unsigned int bitmask = (1 << intr_no);
// asm volatile (
// "CSRR t0, mie; \
// mv t2, %[bitmask]; \
// OR t0, t0, t2; \
// CSRRS x0, mie, t0"
// : /* no output operands */
// : [bitmask] "r" (bitmask)
// : "t0", "t2"
// );
// *((volatile unsigned int*)0x1a109000) |= (1 << intr_no);
gps_handle->CONTROL_REG |= (1 << 2); // acq ip bit pos 2
}
void acq_start(volatile gps_acq_t* gps_handler, unsigned char prn_no) // both acq-tck start and expect bits from interrupt handler.
{
gps_handler->CONTROL_REG = (1u << GPS_ACQ_INTPRT_ENABLE) |
(1u << GPS_ACQ_INTPRT_CLR);
(void)gps_handler->CONTROL_REG;
gps_handler->PRN_NUMBER = prn_no;
(void)gps_handler->PRN_NUMBER;
gps_handler->CONTROL_REG = (1u << GPS_ACQ_INTPRT_ENABLE) |
(1u << GPS_ACQ_START);
//asm volatile ("wfi"); // wait for acq interrupts to be completed.
}
void trck_stop(volatile gps_trck_t* trck_handler)
{
trck_handler->PRN_NUMBER = 0;
trck_handler->CODE_PHASE = 0;
trck_handler->DOPPLER_REG = 0;
trck_handler->CONTROL_REG |= (1 << GPS_TRCK_STOP_POS);
}
void trck_enable_intr(volatile gps_trck_t* gps_handle, unsigned char intr_no)
{
(void)intr_no;
// unsigned int bitmask = (1 << intr_no);
// asm volatile (
// "CSRR t0, mie; \
// mv t2, %[bitmask]; \
// OR t0, t0, t2; \
// CSRRS x0, mie, t0"
// : /* no output operands */
// : [bitmask] "r" (bitmask)
// : "t0", "t2"
// );
// *((volatile unsigned int*)0x1a109000) |= (1 << intr_no);
gps_handle->CONTROL_REG |= (1 << GPS_TRCK_INTR_EN_POS);
}
void trck_disable_intr(volatile gps_trck_t* gps_handle, unsigned char intr_no)
{
(void)intr_no;
// unsigned int bitmask = (1 << intr_no);
// asm volatile (
// "CSRR t0, mie; \
// mv t2, %[bitmask]; \
// AND t0, t0, t2; \
// CSRRS x0, mie, t0"
// : /* no output operands */
// : [bitmask] "r" (~bitmask)
// : "t0", "t2"
// );
// *((volatile unsigned int*)0x1a109000) &= ~bitmask;
gps_handle->CONTROL_REG &= ~(1 << GPS_TRCK_INTR_EN_POS);
}
// set timeout value for stop.
void set_timeout(volatile gps_trck_t* gps_handle, unsigned int timeout_val)
{
gps_handle->TIMEOUT_REG = timeout_val;
}
// parse functions
static unsigned int gps_subframe1_iodc(const unsigned int* subframe)
{
return ((((subframe[2] >> 6) & 0x03u) << 8) |
((subframe[7] >> 22) & 0xffu));
}
static void gps_parse_ephemeris_subframe1(const unsigned int* subframe, gps_ephemeris_t* eph)
{
eph->WN = (subframe[2] >> 20) & 0x3ffu;
eph->tow.week = (int)eph->WN;
eph->code = (unsigned char)((subframe[2] >> 18) & 0x03u);
eph->sva = (unsigned char)((subframe[2] >> 14) & 0x0fu);
eph->svhlth = (unsigned char)((subframe[2] >> 8) & 0x3fu);
eph->iodc = gps_subframe1_iodc(subframe);
eph->l2_data_flag = (unsigned char)((subframe[3] >> 28) & 0x01u);
int8_t tgd_raw = (int8_t)((subframe[6] >> 6) & 0xffu);
eph->tgd = (tgd_raw == INT8_MIN) ? 0.0 : ((double)tgd_raw * P2_31);
/* Toc is the 16-bit field in word 8 scaled by 2^4; parity is excluded. */
eph->toc.sec = (double)(((subframe[7] >> 6) & 0xffffu) << 4);
eph->af2 = (double)(int8_t)((subframe[8] >> 22) & 0xffu) * P2_55;
eph->af1 = (double)(int16_t)((subframe[8] >> 6) & 0xffffu) * P2_43;
eph->af0 = (double)gps_sign_extend((subframe[9] >> 8) & 0x3fffffu, 22u) * P2_31;
}
static void gps_parse_ephemeris_subframe2(const unsigned int* subframe, gps_ephemeris_t* eph)
{
eph->iode = (unsigned char)((subframe[2] >> 22) & 0xffu);
eph->crs = (double)(int16_t)((subframe[2] >> 6) & 0xffffu) * P2_5;
eph->deltan = (double)(int16_t)((subframe[3] >> 14) & 0xffffu) * P2_43 * SC2RAD;
eph->m0 = (double)(int32_t)((((subframe[3] >> 6) & 0xffu) << 24) |
((subframe[4] >> 6) & 0xffffffu)) * P2_31 * SC2RAD;
eph->cuc = (double)(int16_t)((subframe[5] >> 14) & 0xffffu) * P2_29;
eph->ecc = (double)((((subframe[5] >> 6) & 0xffu) << 24) |
((subframe[6] >> 6) & 0xffffffu)) * P2_33;
eph->cus = (double)(int16_t)((subframe[7] >> 14) & 0xffffu) * P2_29;
eph->sqrta = (double)((((subframe[7] >> 6) & 0xffu) << 24) |
((subframe[8] >> 6) & 0xffffffu)) * P2_19;
eph->A = eph->sqrta * eph->sqrta;
eph->toe.sec = (double)(((subframe[9] >> 14) & 0xffffu) << 4);
}
static void gps_parse_ephemeris_subframe3(const unsigned int* subframe, gps_ephemeris_t* eph)
{
eph->cic = (double)(int16_t)((subframe[2] >> 14) & 0xffffu) * P2_29;
eph->omgdot = (double)(int32_t)((((subframe[2] >> 6) & 0xffu) << 24) |
((subframe[3] >> 6) & 0xffffffu)) * P2_31 * SC2RAD;
eph->cis = (double)(int16_t)((subframe[4] >> 14) & 0xffffu) * P2_29;
eph->inc0 = (double)(int32_t)((((subframe[4] >> 6) & 0xffu) << 24) |
((subframe[5] >> 6) & 0xffffffu)) * P2_31 * SC2RAD;
eph->crc = (double)(int16_t)((subframe[6] >> 14) & 0xffffu) * P2_5;
eph->omg0 = (double)(int32_t)((((subframe[6] >> 6) & 0xffu) << 24) |
((subframe[7] >> 6) & 0xffffffu)) * P2_31 * SC2RAD;
eph->omgkdot = (double)gps_sign_extend((subframe[8] >> 6) & 0xffffffu, 24u) * P2_43 * SC2RAD;
eph->idot = (double)gps_sign_extend((subframe[9] >> 8) & 0x3fffu, 14u) * P2_43 * SC2RAD;
eph->update = 0u;
}
static void gps_stage_ephemeris_subframe(const unsigned int* subframe,
gps_frame_t* frame,
unsigned char subframe_id)
{
unsigned int slot = (unsigned int)(subframe_id - 1u);
for(unsigned int i = 0 ; i < 10u ; i++)
{
frame->pending_ephemeris_words[slot][i] = subframe[i];
}
frame->pending_ephemeris_mask |= (unsigned char)(1u << slot);
if((frame->pending_ephemeris_mask & 0x07u) != 0x07u)
{
return;
}
const unsigned int* sf1 = frame->pending_ephemeris_words[0];
const unsigned int* sf2 = frame->pending_ephemeris_words[1];
const unsigned int* sf3 = frame->pending_ephemeris_words[2];
unsigned int iodc = gps_subframe1_iodc(sf1);
unsigned char iode2 = (unsigned char)((sf2[2] >> 22) & 0xffu);
unsigned char iode3 = (unsigned char)((sf3[9] >> 22) & 0xffu);
if(((unsigned char)iodc != iode2) || (iode2 != iode3))
{
GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV,
"ephemeris set waiting for matching IOD: iodc=%u iode2=%u iode3=%u\r\n",
iodc, iode2, iode3);
return;
}
gps_parse_ephemeris_subframe1(sf1, &frame->gps_ephems);
gps_parse_ephemeris_subframe2(sf2, &frame->gps_ephems);
gps_parse_ephemeris_subframe3(sf3, &frame->gps_ephems);
frame->gps_ephems.ephemeris_valid = true;
frame->pending_ephemeris_mask = 0u;
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, "consistent ephemeris set committed: iode=%u\r\n", iode2);
if(frame->gps_ephems.svhlth != 0u)
{
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"unhealthy SV excluded from navigation: svid=%u health=0x%x\r\n",
frame->gps_ephems.SVID, frame->gps_ephems.svhlth);
}
}
unsigned char parse_subframe(int unsigned* subframe, gps_frame_t* gps_frame,
unsigned int* how_tow_sec_out)
{
unsigned char subframe_id = (unsigned char)((subframe[1] >> 8) & 0x07);
unsigned int how_tow = (subframe[1] >> 13) * 6u;
if(how_tow_sec_out != NULL)
{
*how_tow_sec_out = how_tow;
}
if((subframe_id >= 1u) && (subframe_id <= 3u))
{
gps_stage_ephemeris_subframe(subframe, gps_frame, subframe_id);
}
if(subframe_id == 4u)
{
unsigned char data_id = (unsigned char)((subframe[2] >> 28) & 0x03u);
unsigned char sv_id = (unsigned char)((subframe[2] >> 22) & 0x3fu);
unsigned char page = gps_sf4_page_from_sv_id(sv_id);
gps_frame->gps_ephems.sf4_data_id = data_id;
gps_frame->gps_ephems.sf4_sv_id = sv_id;
gps_frame->gps_ephems.sf4_page = page;
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
"subframe 4 parsed: data_id=%u sv_id=%u page=%u\r\n",
data_id, sv_id, page);
/* Ionosphere/UTC fields exist only in LNAV data set 1, page 18 (SV ID 56). */
if((data_id == 1u) && (sv_id == 56u))
{
gps_parse_subframe4_page18(subframe, &gps_frame->gps_ephems);
}
}
if(subframe_id == 5u)
{
unsigned char data_id = (unsigned char)((subframe[2] >> 28) & 0x03u);
unsigned char sv_id = (unsigned char)((subframe[2] >> 22) & 0x3fu);
unsigned char page = gps_sf5_page_from_sv_id(sv_id);
gps_frame->gps_ephems.sf5_data_id = data_id;
gps_frame->gps_ephems.sf5_sv_id = sv_id;
gps_frame->gps_ephems.sf5_page = page;
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
"subframe 5 parsed: data_id=%u sv_id=%u page=%u\r\n",
data_id, sv_id, page);
/* Page 25 carries the almanac reference time/week used by pages 1..24. */
if((data_id == 1u) && (sv_id == 51u))
{
unsigned int toa_raw = (subframe[2] >> 14) & 0xffu;
unsigned int wna = (subframe[2] >> 6) & 0xffu;
gps_frame->gps_ephems.almanac_toa.sec = (double)(toa_raw << 12);
gps_frame->gps_ephems.almanac_wn = wna;
gps_frame->gps_ephems.almanac_ref_valid = true;
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
"subframe 5 page 25 almanac reference parsed: toa=%.0f wna=%u\r\n",
gps_frame->gps_ephems.almanac_toa.sec, wna);
}
}
return subframe_id;
}
static bool gps_tracking_ip_alive(unsigned char trck_ip_no)
{
@@ -546,15 +175,35 @@ static bool gps_ephemeris_ready_for_position(
const gps_ephemeris_t* eph,
unsigned int expected_svid)
{
return (eph != NULL) &&
eph->ephemeris_valid &&
(eph->SVID == expected_svid) &&
(eph->update == 0u) &&
(eph->svhlth == 0u) &&
(eph->sva < 15u) &&
(eph->A > 0.0) &&
(eph->ecc >= 0.0) &&
(eph->ecc < 1.0);
return gps_ephemeris_check_static(eph, expected_svid) ==
GPS_EPHEMERIS_OK;
}
/*
* Same structural rules plus the IS-GPS-200 curve fit window evaluated at the
* epoch's own transmit time. This is what keeps a cached or flash-restored
* ephemeris from being used past its fit interval once receiver time exists.
*/
static bool gps_ephemeris_ready_for_position_at(
const gps_ephemeris_t* eph,
unsigned int expected_svid,
int week,
double tow_sec)
{
gps_ephemeris_status_t status = gps_ephemeris_check(
eph, expected_svid, true, week, tow_sec);
if(status != GPS_EPHEMERIS_OK)
{
gps_report_ephemeris_rejection(expected_svid,
(unsigned int)status);
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"ephemeris rejected for position: prn=%u reason=%s iode=%u toe=%.0f tow=%.3f\r\n",
expected_svid, gps_ephemeris_status_name(status),
(eph != NULL) ? eph->iode : 0u,
(eph != NULL) ? eph->toe.sec : 0.0,
tow_sec);
}
return status == GPS_EPHEMERIS_OK;
}
static bool gps_node_ready_for_position(volatile gps_ephem_ll_t* node)
@@ -703,8 +352,9 @@ static unsigned int gps_collect_completed_epoch_locked(
continue;
}
const gps_ephemeris_t* eph = &epoch->ephemeris[ip_no];
if(!gps_ephemeris_ready_for_position(
eph, epoch->prn[ip_no]) ||
if(!gps_ephemeris_ready_for_position_at(
eph, epoch->prn[ip_no],
epoch->tow[ip_no].week, epoch->tow[ip_no].sec) ||
gps_snapshot_has_svid(snapshot, count, eph->SVID))
{
continue;
@@ -797,7 +447,7 @@ static unsigned int gps_select_coherent_nav_snapshot(
snapshot_measurements[source];
}
snapshot[destination].abs_sample =
gps_unwrap_corrected_abs_sample(
gps_nav_epoch_unwrap_sample(
snapshot_raw_samples[destination],
corrected_sample,
reference_sample);
@@ -1527,6 +1177,22 @@ bool calculate_position(gps_nav_solution_t* solution_out,
tracked_count = gps_collect_tracked_channels_locked(
GPS_DEBUG_ENABLED(DEBUG_CHANNELS) ? tracked_prns : NULL);
/*
* Enable nav solve on every individually qualified channel, whatever the
* total count. Gating this on already holding the WLS minimum was a
* circular dependency: a channel cannot contribute to an epoch until its
* nav-solve interrupt is enabled, and the enable only ran once four
* channels were already contributing. Below four satellites the receiver
* therefore could not climb back out, which is what stalled recovery after
* a total outage for minutes at a time.
*
* gps_enable_nav_solve_for_snapshot_locked() qualifies each channel on its
* own merits - committed ephemeris, alive, fresh measurement - so enabling
* one early is safe. Epochs formed below the minimum simply complete and
* are counted as nav_epoch_below_minimum_total rather than producing a fix.
*/
gps_enable_nav_solve_for_snapshot_locked(snapshot_ips, ready_count);
if(ready_count < GPS_NAV_MIN_WLS_SATELLITES)
{
gps_initial_position_valid = false;
@@ -1535,7 +1201,6 @@ bool calculate_position(gps_nav_solution_t* solution_out,
{
/* The nav-bit snapshot is used only to qualify ephemeris and select the
* channels. Position input must come from a coherent nav-solve epoch. */
gps_enable_nav_solve_for_snapshot_locked(snapshot_ips, ready_count);
if(nav_period_ready)
{
candidate_count = gps_collect_completed_epoch_locked(
@@ -1596,12 +1261,43 @@ bool calculate_position(gps_nav_solution_t* solution_out,
gps_runtime_diagnostics_t status_diagnostics;
gps_get_runtime_diagnostics(&status_diagnostics);
GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV,
"tracked channels=%u ready channels=%u coherent_nav=%u incomplete=%u snapshot_overwrite=%u pps_discontinuity=%u how_reject=%u\r\n",
"tracked channels=%u ready channels=%u coherent_nav=%u incomplete=%u snapshot_overwrite=%u pps_discontinuity=%u how_reject=%u eph_hit=%u eph_miss=%u eph_store=%u eph_reject=%u/%s\r\n",
tracked_count, ready_count, coherent_nav_count,
status_diagnostics.nav_epoch_incomplete_total,
status_diagnostics.nav_epoch_snapshot_overwrite_total,
status_diagnostics.pps_sequence_discontinuity_total,
status_diagnostics.how_anchor_rejected_total);
status_diagnostics.how_anchor_rejected_total,
status_diagnostics.ephemeris_cache_restore_total,
status_diagnostics.ephemeris_cache_miss_total,
status_diagnostics.ephemeris_cache_store_total,
status_diagnostics.ephemeris_rejected_total,
gps_ephemeris_status_name(
(gps_ephemeris_status_t)status_diagnostics
.last_ephemeris_reject_reason));
/*
* The epoch funnel, at status level. Each stage is where a nav epoch
* can die: channels ready for position -> channels captured into a
* complete epoch -> channels surviving the coherence test -> a fix.
* Printing only the endpoints hides which stage drops the epochs,
* which is what made an earlier stall take three wrong diagnoses.
*/
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
"epoch funnel: tracked=%u ready=%u candidates=%u coherent=%u fix=%u | assembly_failed=%u below_min=%u incoherent=%u overwrite=%u deadline=%u restart=%u silenced=%u reseed=%u spread_max=%u how_reject=%u eph_reject=%u eph_hit=%u eph_miss=%u\r\n",
tracked_count, ready_count, candidate_count,
coherent_nav_count, run_nav ? 1u : 0u,
status_diagnostics.nav_epoch_assembly_failed_total,
status_diagnostics.nav_epoch_below_minimum_total,
gps_nav_epoch_reject_total,
status_diagnostics.nav_epoch_snapshot_overwrite_total,
status_diagnostics.nav_epoch_deadline_discard_total,
status_diagnostics.nav_epoch_restart_discard_total,
status_diagnostics.nav_epoch_silenced_total,
status_diagnostics.nav_epoch_reseed_total,
status_diagnostics.nav_epoch_spread_max_ticks,
status_diagnostics.how_anchor_rejected_total,
status_diagnostics.ephemeris_rejected_total,
status_diagnostics.ephemeris_cache_restore_total,
status_diagnostics.ephemeris_cache_miss_total);
last_nav_status_tracked_count = tracked_count;
last_nav_status_ready_count = ready_count;
last_nav_status_coherent_count = coherent_nav_count;
@@ -1739,6 +1435,33 @@ void trck_disable_nav_solve(volatile gps_trck_t* trck_ip)
}
/*
* Hand the tracking IP a navigation-bit phase so nav-solve epochs can start
* before any subframe has been decoded. Hardware answers the arm bit in one
* cycle and clears it, so a seed can never survive to a later reacquisition;
* an out-of-range value is refused by the IP rather than aliased onto the
* wrong grid. Write the value before arming.
*/
void trck_arm_nav_solve_phase_seed(volatile gps_trck_t* trck_ip,
uint32_t phase_seed)
{
trck_ip->NAV_SOLVE_PHASE_SEED = phase_seed;
trck_ip->CONTROL_REG |= (1u << GPS_TRCK_NAV_SOLVE_SEED_ARM_POS);
}
/* True while epochs run on a seed that no decoded subframe has confirmed. */
bool trck_nav_solve_provisional(volatile gps_trck_t* trck_ip)
{
return (trck_ip->STATUS_REG &
(1u << GPS_TRCK_NAV_SOLVE_PROVISIONAL_POS)) != 0u;
}
bool trck_nav_solve_seed_pending(volatile gps_trck_t* trck_ip)
{
return (trck_ip->CONTROL_REG &
(1u << GPS_TRCK_NAV_SOLVE_SEED_ARM_POS)) != 0u;
}
void trck_enable_nav_solve_intr(volatile gps_trck_t* trck_ip)
{
trck_ip->CONTROL_REG |= (1 << GPS_TRCK_NAV_SOLVE_INTR_EN_POS);
@@ -1853,8 +1576,7 @@ void gps_startup_initialization(unsigned int nsp_tick)
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_lnav_reset_ionoutc();
gps_nav_epoch_reject_total = 0u;
for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
{
soc/software/gps_system/src/gps/gps_lnav_parser.c
+671
View File
@@ -0,0 +1,671 @@
/*
* LNAV navigation-message decoder.
*
* Split out of gps_ip.c so the decoder can be linked into host tests and into
* the persistent ephemeris store without dragging in MMIO or the tracking
* driver. This is deliberately the same code the receiver runs: a restored
* flash record is parsed by exactly the decoder that produced it.
*/
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "gps/gps_ip_defines.h"
#include "gps/Navigation.h"
#include "gps/gps_time_anchor.h"
/*
* Subframe 4 page 18 ionosphere/UTC parameters are receiver-global, not
* per-channel, so the decoder owns the latest copy.
*/
static volatile bool gps_ionoutc_valid = false;
static ionoutc_t gps_last_ionoutc;
/*
* The almanac is receiver-global for the same reason the ionosphere model is:
* every satellite broadcasts the whole constellation, so whichever channel
* decodes a page first fills in that satellite for everyone.
*/
static gps_almanac_t gps_lnav_almanac;
/*
* Raw LNAV words for the pages worth persisting. The parsed forms are kept
* too, but the raw words are what goes to flash: the on-flash format then does
* not depend on compiler layout, and a restored page is decoded by exactly the
* decoder that produced it - the same rule the ephemeris store follows.
*/
static uint32_t gps_lnav_iono_words[10];
static bool gps_lnav_iono_words_valid;
static uint32_t gps_lnav_almanac_words[GPS_ALMANAC_PRN_COUNT][10];
static uint32_t gps_lnav_almanac_words_mask;
static void gps_parse_subframe4_page18(const unsigned int* subframe,
gps_ephemeris_t* eph);
const gps_almanac_t* gps_lnav_get_almanac(void)
{
return &gps_lnav_almanac;
}
void gps_lnav_reset_almanac(void)
{
gps_almanac_reset(&gps_lnav_almanac);
gps_lnav_almanac_words_mask = 0u;
memset(gps_lnav_almanac_words, 0, sizeof(gps_lnav_almanac_words));
}
static void gps_lnav_capture_words(uint32_t dst[10],
const unsigned int* subframe)
{
for(unsigned int i = 0u; i < 10u; i++)
{
dst[i] = (uint32_t)subframe[i];
}
}
bool gps_lnav_get_iono_page(uint32_t words_out[10])
{
if((words_out == NULL) || !gps_lnav_iono_words_valid)
{
return false;
}
memcpy(words_out, gps_lnav_iono_words, sizeof(gps_lnav_iono_words));
return true;
}
uint32_t gps_lnav_get_almanac_pages(
uint32_t words_out[GPS_ALMANAC_PRN_COUNT][10])
{
if(words_out == NULL)
{
return 0u;
}
memcpy(words_out, gps_lnav_almanac_words,
sizeof(gps_lnav_almanac_words));
return gps_lnav_almanac_words_mask;
}
bool gps_lnav_restore_iono_page(const unsigned int* subframe)
{
if(subframe == NULL)
{
return false;
}
/*
* Re-parse rather than trusting a stored struct. A page that does not
* decode is simply not adopted, so a corrupt or stale flash record cannot
* install a bogus ionosphere model.
*/
gps_ephemeris_t scratch;
memset(&scratch, 0, sizeof(scratch));
gps_parse_subframe4_page18(subframe, &scratch);
if(!gps_ionoutc_valid)
{
return false;
}
/*
* Re-seed the raw-word cache as well, not just the parsed model. The next
* save rebuilds the payload from this cache, so without it a restored
* ionosphere page would be written back as absent and the stored model
* would be lost on the first save after every reboot - defeating the
* persistence entirely until page 18 came round again 12.5 minutes later.
*/
gps_lnav_capture_words(gps_lnav_iono_words, subframe);
gps_lnav_iono_words_valid = true;
return true;
}
bool gps_lnav_restore_almanac_page(const unsigned int* subframe,
unsigned char subframe_id)
{
if(subframe == NULL)
{
return false;
}
if(!gps_almanac_store_page(&gps_lnav_almanac, subframe, subframe_id))
{
return false;
}
unsigned char sv_id =
(unsigned char)((subframe[2] >> 22) & 0x3fu);
if((sv_id >= 1u) && (sv_id <= GPS_ALMANAC_PRN_COUNT))
{
gps_lnav_capture_words(gps_lnav_almanac_words[sv_id - 1u], subframe);
gps_lnav_almanac_words_mask |= (1u << (sv_id - 1u));
}
return true;
}
void gps_lnav_restore_almanac_reference(unsigned int week, double toa_sec)
{
gps_almanac_set_reference(&gps_lnav_almanac, week, toa_sec);
}
bool gps_get_ionoutc(ionoutc_t* ionoutc)
{
if((ionoutc == NULL) || !gps_ionoutc_valid)
{
return false;
}
*ionoutc = gps_last_ionoutc;
return true;
}
void gps_lnav_reset_ionoutc(void)
{
gps_ionoutc_valid = false;
gps_last_ionoutc = (ionoutc_t){0};
}
static int32_t gps_sign_extend(uint32_t value, unsigned int bits)
{
if((bits == 0u) || (bits >= 32u))
{
return (int32_t)value;
}
uint32_t sign = 1u << (bits - 1u);
return (int32_t)((value ^ sign) - sign);
}
static unsigned char gps_sf4_page_from_sv_id(unsigned char sv_id)
{
if((sv_id >= 25u) && (sv_id <= 28u))
{
return (unsigned char)(sv_id - 23u); /* pages 2..5 */
}
if((sv_id >= 29u) && (sv_id <= 32u))
{
return (unsigned char)(sv_id - 22u); /* pages 7..10 */
}
switch(sv_id)
{
case 52u: return 13u;
case 53u: return 14u;
case 54u: return 15u;
case 55u: return 17u;
case 56u: return 18u;
case 58u: return 19u;
case 59u: return 20u;
case 60u: return 22u;
case 61u: return 23u;
case 63u: return 25u;
default: return 0u;
}
}
static unsigned char gps_sf5_page_from_sv_id(unsigned char sv_id)
{
if((sv_id >= 1u) && (sv_id <= 24u))
{
return sv_id;
}
return (sv_id == 51u) ? 25u : 0u;
}
static void gps_parse_subframe4_page18(const unsigned int* subframe, gps_ephemeris_t* eph)
{
ionoutc_t* ion = &eph->ionoutc;
uint32_t word3 = subframe[2];
uint32_t word4 = subframe[3];
uint32_t word5 = subframe[4];
uint32_t word6 = subframe[5];
uint32_t word7 = subframe[6];
uint32_t word8 = subframe[7];
uint32_t word9 = subframe[8];
uint32_t word10 = subframe[9];
/* The IP has already undone LNAV D30* data inversion. These casts only
* interpret the extracted ICD signed fields at their specified width. */
ion->alpha0 = (double)(int8_t)((word3 >> 14) & 0xffu) * P2_30;
ion->alpha1 = (double)(int8_t)((word3 >> 6) & 0xffu) * P2_27;
ion->alpha2 = (double)(int8_t)((word4 >> 22) & 0xffu) * P2_24;
ion->alpha3 = (double)(int8_t)((word4 >> 14) & 0xffu) * P2_24;
ion->beta0 = (double)(int8_t)((word4 >> 6) & 0xffu) * 2048.0;
ion->beta1 = (double)(int8_t)((word5 >> 22) & 0xffu) * 16384.0;
ion->beta2 = (double)(int8_t)((word5 >> 14) & 0xffu) * 65536.0;
ion->beta3 = (double)(int8_t)((word5 >> 6) & 0xffu) * 65536.0;
ion->A1 = (double)gps_sign_extend((word6 >> 6) & 0xffffffu, 24u) * P2_50;
uint32_t a0_raw = (((word7 >> 6) & 0xffffffu) << 8) | ((word8 >> 22) & 0xffu);
int64_t a0_signed = (a0_raw & 0x80000000u) != 0u
? (int64_t)a0_raw - INT64_C(4294967296)
: (int64_t)a0_raw;
ion->A0 = (double)a0_signed * P2_30;
ion->tot = (int)(((word8 >> 14) & 0xffu) << 12);
ion->wnt = (int)((word8 >> 6) & 0xffu);
ion->dtls = (int)(int8_t)((word9 >> 22) & 0xffu);
ion->wnlsf = (int)((word9 >> 14) & 0xffu);
ion->dn = (int)((word9 >> 6) & 0xffu);
ion->dtlsf = (int)(int8_t)((word10 >> 22) & 0xffu);
ion->enable = true;
ion->vflg = true;
gps_last_ionoutc = *ion;
gps_ionoutc_valid = true;
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
"subframe 4 page 18 iono parsed: a=[%.3e %.3e %.3e %.3e] b=[%.3e %.3e %.3e %.3e]\r\n",
ion->alpha0, ion->alpha1, ion->alpha2, ion->alpha3,
ion->beta0, ion->beta1, ion->beta2, ion->beta3);
}
void acq_afe_start(volatile gps_acq_t* gps_handle)
{
gps_handle->CONTROL_REG |= (1 << GPS_ACQ_AFE_START);
}
void acq_enable_intr(volatile gps_acq_t* gps_handle, unsigned char intr_no)
{
(void)intr_no;
// unsigned int bitmask = (1 << intr_no);
// asm volatile (
// "CSRR t0, mie; \
// mv t2, %[bitmask]; \
// OR t0, t0, t2; \
// CSRRS x0, mie, t0"
// : /* no output operands */
// : [bitmask] "r" (bitmask)
// : "t0", "t2"
// );
// *((volatile unsigned int*)0x1a109000) |= (1 << intr_no);
gps_handle->CONTROL_REG |= (1 << 2); // acq ip bit pos 2
}
void acq_start(volatile gps_acq_t* gps_handler, unsigned char prn_no) // both acq-tck start and expect bits from interrupt handler.
{
gps_handler->CONTROL_REG = (1u << GPS_ACQ_INTPRT_ENABLE) |
(1u << GPS_ACQ_INTPRT_CLR);
(void)gps_handler->CONTROL_REG;
gps_handler->PRN_NUMBER = prn_no;
(void)gps_handler->PRN_NUMBER;
gps_handler->CONTROL_REG = (1u << GPS_ACQ_INTPRT_ENABLE) |
(1u << GPS_ACQ_START);
//asm volatile ("wfi"); // wait for acq interrupts to be completed.
}
void trck_stop(volatile gps_trck_t* trck_handler)
{
trck_handler->PRN_NUMBER = 0;
trck_handler->CODE_PHASE = 0;
trck_handler->DOPPLER_REG = 0;
trck_handler->CONTROL_REG |= (1 << GPS_TRCK_STOP_POS);
}
void trck_enable_intr(volatile gps_trck_t* gps_handle, unsigned char intr_no)
{
(void)intr_no;
// unsigned int bitmask = (1 << intr_no);
// asm volatile (
// "CSRR t0, mie; \
// mv t2, %[bitmask]; \
// OR t0, t0, t2; \
// CSRRS x0, mie, t0"
// : /* no output operands */
// : [bitmask] "r" (bitmask)
// : "t0", "t2"
// );
// *((volatile unsigned int*)0x1a109000) |= (1 << intr_no);
gps_handle->CONTROL_REG |= (1 << GPS_TRCK_INTR_EN_POS);
}
void trck_disable_intr(volatile gps_trck_t* gps_handle, unsigned char intr_no)
{
(void)intr_no;
// unsigned int bitmask = (1 << intr_no);
// asm volatile (
// "CSRR t0, mie; \
// mv t2, %[bitmask]; \
// AND t0, t0, t2; \
// CSRRS x0, mie, t0"
// : /* no output operands */
// : [bitmask] "r" (~bitmask)
// : "t0", "t2"
// );
// *((volatile unsigned int*)0x1a109000) &= ~bitmask;
gps_handle->CONTROL_REG &= ~(1 << GPS_TRCK_INTR_EN_POS);
}
// set timeout value for stop.
void set_timeout(volatile gps_trck_t* gps_handle, unsigned int timeout_val)
{
gps_handle->TIMEOUT_REG = timeout_val;
}
// parse functions
static unsigned int gps_subframe1_iodc(const unsigned int* subframe)
{
return ((((subframe[2] >> 6) & 0x03u) << 8) |
((subframe[7] >> 22) & 0xffu));
}
static void gps_parse_ephemeris_subframe1(const unsigned int* subframe, gps_ephemeris_t* eph)
{
eph->WN = (subframe[2] >> 20) & 0x3ffu;
eph->tow.week = (int)eph->WN;
eph->code = (unsigned char)((subframe[2] >> 18) & 0x03u);
eph->sva = (unsigned char)((subframe[2] >> 14) & 0x0fu);
eph->svhlth = (unsigned char)((subframe[2] >> 8) & 0x3fu);
eph->iodc = gps_subframe1_iodc(subframe);
eph->l2_data_flag = (unsigned char)((subframe[3] >> 28) & 0x01u);
int8_t tgd_raw = (int8_t)((subframe[6] >> 6) & 0xffu);
eph->tgd = (tgd_raw == INT8_MIN) ? 0.0 : ((double)tgd_raw * P2_31);
/* Toc is the 16-bit field in word 8 scaled by 2^4; parity is excluded. */
eph->toc.sec = (double)(((subframe[7] >> 6) & 0xffffu) << 4);
eph->af2 = (double)(int8_t)((subframe[8] >> 22) & 0xffu) * P2_55;
eph->af1 = (double)(int16_t)((subframe[8] >> 6) & 0xffffu) * P2_43;
eph->af0 = (double)gps_sign_extend((subframe[9] >> 8) & 0x3fffffu, 22u) * P2_31;
}
static void gps_parse_ephemeris_subframe2(const unsigned int* subframe, gps_ephemeris_t* eph)
{
eph->iode = (unsigned char)((subframe[2] >> 22) & 0xffu);
eph->crs = (double)(int16_t)((subframe[2] >> 6) & 0xffffu) * P2_5;
eph->deltan = (double)(int16_t)((subframe[3] >> 14) & 0xffffu) * P2_43 * SC2RAD;
eph->m0 = (double)(int32_t)((((subframe[3] >> 6) & 0xffu) << 24) |
((subframe[4] >> 6) & 0xffffffu)) * P2_31 * SC2RAD;
eph->cuc = (double)(int16_t)((subframe[5] >> 14) & 0xffffu) * P2_29;
eph->ecc = (double)((((subframe[5] >> 6) & 0xffu) << 24) |
((subframe[6] >> 6) & 0xffffffu)) * P2_33;
eph->cus = (double)(int16_t)((subframe[7] >> 14) & 0xffffu) * P2_29;
eph->sqrta = (double)((((subframe[7] >> 6) & 0xffu) << 24) |
((subframe[8] >> 6) & 0xffffffu)) * P2_19;
eph->A = eph->sqrta * eph->sqrta;
eph->toe.sec = (double)(((subframe[9] >> 14) & 0xffffu) << 4);
/* Word 10 bit 17 (bit 13 once the six parity bits are counted) is the
* curve fit interval flag; it selects between the nominal four hour fit
* and the extended intervals of IS-GPS-200 Table 20-XII. */
eph->fit_flag = (((subframe[9] >> 13) & 0x01u) != 0u);
}
static void gps_parse_ephemeris_subframe3(const unsigned int* subframe, gps_ephemeris_t* eph)
{
eph->cic = (double)(int16_t)((subframe[2] >> 14) & 0xffffu) * P2_29;
eph->omgdot = (double)(int32_t)((((subframe[2] >> 6) & 0xffu) << 24) |
((subframe[3] >> 6) & 0xffffffu)) * P2_31 * SC2RAD;
eph->cis = (double)(int16_t)((subframe[4] >> 14) & 0xffffu) * P2_29;
eph->inc0 = (double)(int32_t)((((subframe[4] >> 6) & 0xffu) << 24) |
((subframe[5] >> 6) & 0xffffffu)) * P2_31 * SC2RAD;
eph->crc = (double)(int16_t)((subframe[6] >> 14) & 0xffffu) * P2_5;
eph->omg0 = (double)(int32_t)((((subframe[6] >> 6) & 0xffu) << 24) |
((subframe[7] >> 6) & 0xffffffu)) * P2_31 * SC2RAD;
eph->omgkdot = (double)gps_sign_extend((subframe[8] >> 6) & 0xffffffu, 24u) * P2_43 * SC2RAD;
eph->idot = (double)gps_sign_extend((subframe[9] >> 8) & 0x3fffu, 14u) * P2_43 * SC2RAD;
eph->update = 0u;
}
/*
* Give Toe a week.
*
* LNAV broadcasts Toe as a second-of-week; the week it belongs to has to come
* from elsewhere, and the only self-consistent source is the pair (WN from
* subframe 1, handover word of the subframe that carried Toe). Toe is within
* a couple of hours of its own transmission, so whichever week puts it closest
* to that transmission is the right one - and that resolves the two cases the
* ICD allows: a Toe early in the next week broadcast at the end of this one,
* and the reverse.
*
* Without this the curve-fit check in gps_ephemeris_validity.c can only work
* modulo a week, because it has nothing but two second-of-week values to
* compare. A record exactly one week old then reads as brand new - which is
* unreachable while ephemeris lives in RAM and expires in hours, and directly
* reachable once it is persisted to flash across a power-off.
*/
static void gps_resolve_toe_week(const unsigned int* subframe2,
gps_ephemeris_t* eph)
{
eph->toe.week = 0;
eph->toe_week_valid = false;
if((subframe2 == NULL) || (eph == NULL))
{
return;
}
/* Handover word: the 17-bit truncated TOW count of the *next* subframe,
* in units of six seconds. */
double transmit_tow_sec = (double)(subframe2[1] >> 13) * 6.0;
if(!(transmit_tow_sec >= 0.0) ||
(transmit_tow_sec >= GPS_TIME_WEEK_SECONDS) ||
!isfinite(eph->toe.sec))
{
return;
}
int week = eph->tow.week;
double delta = eph->toe.sec - transmit_tow_sec;
if(delta < -GPS_TIME_HALF_WEEK_SECONDS)
{
week += 1; /* Toe is early in the following week. */
}
else if(delta > GPS_TIME_HALF_WEEK_SECONDS)
{
week -= 1; /* the transmission crossed into the following week. */
}
if(week < 0)
{
return;
}
eph->toe.week = week;
eph->toe_week_valid = true;
}
static void gps_stage_ephemeris_subframe(const unsigned int* subframe,
gps_frame_t* frame,
unsigned char subframe_id)
{
unsigned int slot = (unsigned int)(subframe_id - 1u);
for(unsigned int i = 0 ; i < 10u ; i++)
{
frame->pending_ephemeris_words[slot][i] = subframe[i];
}
frame->pending_ephemeris_mask |= (unsigned char)(1u << slot);
if((frame->pending_ephemeris_mask & 0x07u) != 0x07u)
{
return;
}
const unsigned int* sf1 = frame->pending_ephemeris_words[0];
const unsigned int* sf2 = frame->pending_ephemeris_words[1];
const unsigned int* sf3 = frame->pending_ephemeris_words[2];
unsigned int iodc = gps_subframe1_iodc(sf1);
unsigned char iode2 = (unsigned char)((sf2[2] >> 22) & 0xffu);
unsigned char iode3 = (unsigned char)((sf3[9] >> 22) & 0xffu);
if(((unsigned char)iodc != iode2) || (iode2 != iode3))
{
GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV,
"ephemeris set waiting for matching IOD: iodc=%u iode2=%u iode3=%u\r\n",
iodc, iode2, iode3);
return;
}
gps_parse_ephemeris_subframe1(sf1, &frame->gps_ephems);
gps_parse_ephemeris_subframe2(sf2, &frame->gps_ephems);
gps_parse_ephemeris_subframe3(sf3, &frame->gps_ephems);
gps_resolve_toe_week(sf2, &frame->gps_ephems);
frame->gps_ephems.ephemeris_valid = true;
frame->pending_ephemeris_mask = 0u;
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, "consistent ephemeris set committed: iode=%u\r\n", iode2);
if(frame->gps_ephems.svhlth != 0u)
{
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"unhealthy SV excluded from navigation: svid=%u health=0x%x\r\n",
frame->gps_ephems.SVID, frame->gps_ephems.svhlth);
}
}
/*
* Issue-of-data carried by a raw subframe, without staging or committing it.
* Subframe 1 broadcasts the ten-bit IODC whose low eight bits equal the IODE of
* the same data set, so all three subframes reduce to one comparable byte.
* Returns false for subframes that carry no issue of data.
*/
bool gps_lnav_broadcast_iode(const unsigned int* subframe,
unsigned char subframe_id,
unsigned char* iode_out)
{
if((subframe == NULL) || (iode_out == NULL))
{
return false;
}
switch(subframe_id)
{
case 1u:
*iode_out = (unsigned char)(gps_subframe1_iodc(subframe) & 0xffu);
return true;
case 2u:
*iode_out = (unsigned char)((subframe[2] >> 22) & 0xffu);
return true;
case 3u:
*iode_out = (unsigned char)((subframe[9] >> 22) & 0xffu);
return true;
default:
return false;
}
}
unsigned char parse_subframe(int unsigned* subframe, gps_frame_t* gps_frame,
unsigned int* how_tow_sec_out)
{
unsigned char subframe_id = (unsigned char)((subframe[1] >> 8) & 0x07);
unsigned int how_tow = (subframe[1] >> 13) * 6u;
if(how_tow_sec_out != NULL)
{
*how_tow_sec_out = how_tow;
}
if((subframe_id >= 1u) && (subframe_id <= 3u))
{
gps_stage_ephemeris_subframe(subframe, gps_frame, subframe_id);
}
if(subframe_id == 4u)
{
unsigned char data_id = (unsigned char)((subframe[2] >> 28) & 0x03u);
unsigned char sv_id = (unsigned char)((subframe[2] >> 22) & 0x3fu);
unsigned char page = gps_sf4_page_from_sv_id(sv_id);
gps_frame->gps_ephems.sf4_data_id = data_id;
gps_frame->gps_ephems.sf4_sv_id = sv_id;
gps_frame->gps_ephems.sf4_page = page;
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
"subframe 4 parsed: data_id=%u sv_id=%u page=%u\r\n",
data_id, sv_id, page);
/* Ionosphere/UTC fields exist only in LNAV data set 1, page 18 (SV ID 56). */
if((data_id == 1u) && (sv_id == 56u))
{
gps_parse_subframe4_page18(subframe, &gps_frame->gps_ephems);
/* Keep the raw page so it can be persisted verbatim. */
gps_lnav_capture_words(gps_lnav_iono_words, subframe);
gps_lnav_iono_words_valid = true;
}
/*
* SV IDs 25..32 on subframe 4 are almanac pages for those PRNs. The
* range test is repeated here rather than inherited from
* gps_almanac_store_page(): it is what bounds the index below, and an
* invariant that only holds because another function re-derives the
* same field from the same words is one refactor away from a 40 byte
* write past the end of a static array.
*/
if(gps_almanac_store_page(&gps_lnav_almanac, subframe, subframe_id) &&
(sv_id >= 1u) && (sv_id <= GPS_ALMANAC_PRN_COUNT))
{
gps_lnav_capture_words(
gps_lnav_almanac_words[sv_id - 1u], subframe);
gps_lnav_almanac_words_mask |= (1u << (sv_id - 1u));
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
"almanac page stored: prn=%u known=%u\r\n",
sv_id, gps_almanac_valid_count(&gps_lnav_almanac));
}
}
if(subframe_id == 5u)
{
unsigned char data_id = (unsigned char)((subframe[2] >> 28) & 0x03u);
unsigned char sv_id = (unsigned char)((subframe[2] >> 22) & 0x3fu);
unsigned char page = gps_sf5_page_from_sv_id(sv_id);
gps_frame->gps_ephems.sf5_data_id = data_id;
gps_frame->gps_ephems.sf5_sv_id = sv_id;
gps_frame->gps_ephems.sf5_page = page;
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
"subframe 5 parsed: data_id=%u sv_id=%u page=%u\r\n",
data_id, sv_id, page);
/* Page 25 carries the almanac reference time/week used by pages 1..24. */
if((data_id == 1u) && (sv_id == 51u))
{
unsigned int toa_raw = (subframe[2] >> 14) & 0xffu;
unsigned int wna = (subframe[2] >> 6) & 0xffu;
gps_frame->gps_ephems.almanac_toa.sec = (double)(toa_raw << 12);
gps_frame->gps_ephems.almanac_wn = wna;
gps_frame->gps_ephems.almanac_ref_valid = true;
/*
* WNa is the 8 LSBs of the almanac reference week. Lift it into
* the full week using the week this frame has already resolved,
* so almanac propagation shares one timescale with the rest of
* the receiver.
*
* The reference week is tow.week, set from the subframe 1 WN and
* from the HOW anchor. It is emphatically not toe.week: LNAV
* broadcasts Toe as a second-of-week only, nothing ever writes
* that field, and lifting against the zero it holds pushed the
* result 256 weeks below the receiver for every WNa above 128.
* In unsigned arithmetic that underflowed to ~4.29e9, which the
* propagation then had to walk back one week at a time.
*
* Signed arithmetic and a symmetric comparison keep the lift
* inside +/-128 weeks of the receiver by construction; with no
* week to lift against there is nothing to say, so the reference
* is left unset and visibility falls back to assuming the almanac
* is current - which it is, since it was just received.
*/
int frame_week = gps_frame->gps_ephems.tow.week;
int almanac_week = (frame_week & ~0xff) | (int)(wna & 0xffu);
if((almanac_week - frame_week) > 128)
{
almanac_week -= 256;
}
else if((frame_week - almanac_week) > 128)
{
almanac_week += 256;
}
if((frame_week > 0) && (almanac_week >= 0))
{
gps_almanac_set_reference(
&gps_lnav_almanac, (unsigned int)almanac_week,
gps_frame->gps_ephems.almanac_toa.sec);
}
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
"subframe 5 page 25 almanac reference parsed: toa=%.0f wna=%u frame_week=%d week=%d\r\n",
gps_frame->gps_ephems.almanac_toa.sec, wna,
frame_week, almanac_week);
}
/* SV IDs 1..24 on subframe 5 are almanac pages for those PRNs; the
* range test bounds the index, as on subframe 4 above. */
if(gps_almanac_store_page(&gps_lnav_almanac, subframe, subframe_id) &&
(sv_id >= 1u) && (sv_id <= GPS_ALMANAC_PRN_COUNT))
{
gps_lnav_capture_words(
gps_lnav_almanac_words[sv_id - 1u], subframe);
gps_lnav_almanac_words_mask |= (1u << (sv_id - 1u));
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
"almanac page stored: prn=%u known=%u\r\n",
sv_id, gps_almanac_valid_count(&gps_lnav_almanac));
}
}
return subframe_id;
}
soc/software/gps_system/src/gps/gps_nav_context_store.c
+256
View File
@@ -0,0 +1,256 @@
#include "gps/gps_nav_context_store.h"
#include <math.h>
#include <stddef.h>
#include <string.h>
#define GPS_NAV_CONTEXT_PAGE 256u
/*
* Page 0 of a sector holds the header, the payload follows from page 1. The
* header is programmed last so a save torn by power loss leaves a sector whose
* header never validates, and mount falls back to the other sector.
*/
typedef struct {
uint32_t magic;
uint16_t version;
uint16_t reserved;
uint32_t generation;
uint32_t payload_len;
uint32_t payload_crc32;
uint32_t header_crc32;
} gps_nav_context_header_t;
_Static_assert(sizeof(gps_nav_context_header_t) <= GPS_NAV_CONTEXT_PAGE,
"nav context header does not fit one flash page");
static uint32_t context_crc32(const void* data, uint32_t len)
{
/* Same CRC as the ephemeris store, reused rather than reimplemented. */
return gps_ephemeris_store_crc32(data, len);
}
static uint32_t sector_address(const gps_nav_context_store_t* store,
unsigned int sector)
{
return store->flash.base_address +
((sector != 0u) ? store->flash.sector_size : 0u);
}
static bool header_valid(const gps_nav_context_header_t* header)
{
if((header->magic != GPS_NAV_CONTEXT_MAGIC) ||
(header->version != GPS_NAV_CONTEXT_VERSION) ||
(header->payload_len != (uint32_t)sizeof(gps_nav_context_payload_t)))
{
return false;
}
uint32_t expected = context_crc32(
header, (uint32_t)offsetof(gps_nav_context_header_t, header_crc32));
return expected == header->header_crc32;
}
static bool read_sector(const gps_nav_context_store_t* store,
unsigned int sector,
gps_nav_context_header_t* header_out,
gps_nav_context_payload_t* payload_out)
{
uint32_t base = sector_address(store, sector);
store->flash.read(store->flash.ctx, base, header_out,
(uint32_t)sizeof(*header_out));
if(!header_valid(header_out))
{
return false;
}
store->flash.read(store->flash.ctx, base + GPS_NAV_CONTEXT_PAGE,
payload_out, (uint32_t)sizeof(*payload_out));
uint32_t crc = context_crc32(payload_out,
(uint32_t)sizeof(*payload_out));
return crc == header_out->payload_crc32;
}
bool gps_nav_context_store_mount(
gps_nav_context_store_t* store,
const gps_ephemeris_store_flash_t* flash)
{
if((store == NULL) || (flash == NULL) || (flash->read == NULL) ||
(flash->program == NULL) || (flash->erase_sector == NULL) ||
(flash->sector_size < (GPS_NAV_CONTEXT_PAGE +
sizeof(gps_nav_context_payload_t))))
{
return false;
}
memset(store, 0, sizeof(*store));
store->flash = *flash;
gps_nav_context_header_t header;
gps_nav_context_payload_t payload;
bool found = false;
for(unsigned int sector = 0u; sector < 2u; sector++)
{
if(!read_sector(store, sector, &header, &payload))
{
store->rejected++;
continue;
}
if(!found || (header.generation > store->generation))
{
found = true;
store->active_sector = sector;
store->generation = header.generation;
store->payload_crc32 = header.payload_crc32;
}
}
store->mounted = true;
store->has_payload = found;
return true;
}
bool gps_nav_context_store_load(
const gps_nav_context_store_t* store,
gps_nav_context_payload_t* payload_out)
{
if((store == NULL) || !store->mounted || !store->has_payload ||
(payload_out == NULL))
{
return false;
}
gps_nav_context_header_t header;
return read_sector(store, store->active_sector, &header, payload_out);
}
bool gps_nav_context_store_save(
gps_nav_context_store_t* store,
const gps_nav_context_payload_t* payload,
bool* wrote_out)
{
if(wrote_out != NULL)
{
*wrote_out = false;
}
if((store == NULL) || !store->mounted || (payload == NULL))
{
return false;
}
uint32_t crc = context_crc32(payload,
(uint32_t)sizeof(*payload));
/*
* The wear policy. Almanac pages are rebroadcast continuously and the
* ionosphere model changes rarely, so the overwhelming majority of saves
* carry bytes identical to what is already on the flash. Comparing the
* CRC first turns those into no-ops and keeps the erase count tied to how
* often the constellation publishes new data, not to how often this is
* called.
*/
if(store->has_payload && (crc == store->payload_crc32))
{
store->skipped_writes++;
return true;
}
/* Always write into the sector that is not currently live. */
unsigned int target = store->has_payload
? (store->active_sector ^ 1u) : store->active_sector;
uint32_t base = sector_address(store, target);
if(!store->flash.erase_sector(store->flash.ctx, base))
{
return false;
}
store->erases++;
const uint8_t* bytes = (const uint8_t*)payload;
uint32_t remaining = (uint32_t)sizeof(*payload);
uint32_t offset = 0u;
while(remaining != 0u)
{
uint32_t chunk = (remaining > GPS_NAV_CONTEXT_PAGE)
? GPS_NAV_CONTEXT_PAGE : remaining;
if(!store->flash.program(store->flash.ctx,
base + GPS_NAV_CONTEXT_PAGE + offset,
&bytes[offset], chunk))
{
return false;
}
offset += chunk;
remaining -= chunk;
}
gps_nav_context_header_t header;
memset(&header, 0, sizeof(header));
header.magic = GPS_NAV_CONTEXT_MAGIC;
header.version = (uint16_t)GPS_NAV_CONTEXT_VERSION;
header.generation = store->generation + 1u;
header.payload_len = (uint32_t)sizeof(*payload);
header.payload_crc32 = crc;
header.header_crc32 = context_crc32(
&header, (uint32_t)offsetof(gps_nav_context_header_t, header_crc32));
/* Header last: until this lands the sector is not adoptable. */
if(!store->flash.program(store->flash.ctx, base, &header,
(uint32_t)sizeof(header)))
{
return false;
}
store->active_sector = target;
store->generation = header.generation;
store->payload_crc32 = crc;
store->has_payload = true;
store->writes++;
if(wrote_out != NULL)
{
*wrote_out = true;
}
return true;
}
bool gps_nav_context_position_changed(const double persisted_ecef_m[3],
const double candidate_ecef_m[3],
double epsilon_m)
{
if(candidate_ecef_m == NULL)
{
return false;
}
if(persisted_ecef_m == NULL)
{
return true;
}
double squared = 0.0;
for(unsigned int i = 0u; i < 3u; i++)
{
double d = candidate_ecef_m[i] - persisted_ecef_m[i];
if(!isfinite(d))
{
return false;
}
squared += d * d;
}
return squared > (epsilon_m * epsilon_m);
}
bool gps_nav_context_store_format(gps_nav_context_store_t* store)
{
if((store == NULL) || !store->mounted)
{
return false;
}
for(unsigned int sector = 0u; sector < 2u; sector++)
{
if(!store->flash.erase_sector(store->flash.ctx,
sector_address(store, sector)))
{
return false;
}
store->erases++;
}
store->has_payload = false;
store->generation = 0u;
store->active_sector = 0u;
store->payload_crc32 = 0u;
return true;
}
soc/software/gps_system/src/gps/gps_nav_epoch.c
+9
View File
@@ -37,6 +37,15 @@ static bool gps_nav_epoch_sample_after(uint32_t first, uint32_t second)
return (delta != 0u) && (delta < 0x80000000u);
}
double gps_nav_epoch_unwrap_sample(uint32_t raw_sample,
double corrected_sample,
uint32_t reference_sample)
{
int32_t delta = (int32_t)(raw_sample - reference_sample);
double code_phase_correction = (double)raw_sample - corrected_sample;
return (double)reference_sample + (double)delta - code_phase_correction;
}
static unsigned int gps_nav_epoch_mask_count(uint32_t mask)
{
unsigned int count = 0u;
soc/software/gps_system/src/gps/gps_pps.c
+11 -5
View File
@@ -183,6 +183,16 @@ void gps_pps_init(unsigned int nav_solve_tick)
GPS_PPS_CLOCK_HZ));
}
uint64_t gps_pps_read_raw_counter(void)
{
/* Reading LO snapshots HI in hardware, so this is a coherent 64-bit
* free-running counter read over the 32-bit APB interface. The order
* matters: HI must be read after LO. */
uint32_t now_lo = gps_pps->raw_counter_lo;
uint32_t now_hi = gps_pps->raw_counter_hi;
return ((uint64_t)now_hi << 32) | now_lo;
}
void gps_pps_service(void)
{
if(!gps_pps_reference_committed || gps_pps_holdover)
@@ -190,11 +200,7 @@ void gps_pps_service(void)
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;
uint64_t now = gps_pps_read_raw_counter();
if((now - gps_pps_last_reference_counter) >
gps_pps_reference_timeout_ticks)
{
soc/software/gps_system/src/gps/gps_pps_time_filter.c
+14 -8
View File
@@ -72,21 +72,26 @@ bool gps_pps_select_reference(
return false;
}
/*
* Screen the whole set before choosing anything. This used to return as
* soon as the preferred satellite was found, which left any non-finite
* capture time *after* it in the array unexamined - so whether a corrupt
* measurement was caught depended on where the sticky reference PRN
* happened to sit. Every entry becomes a pseudorange, so every entry has
* to be finite for any of them to mean anything.
*/
unsigned int earliest_index = 0u;
if(!isfinite(capture_times[0]))
{
return false;
}
unsigned int preferred_index = count;
for(unsigned int index = 0u; index < count; index++)
{
if(!isfinite(capture_times[index]))
{
return false;
}
if(preferred_valid && (satellite_ids[index] == preferred_id))
if(preferred_valid && (preferred_index == count) &&
(satellite_ids[index] == preferred_id))
{
*reference_index_out = index;
return true;
preferred_index = index;
}
if(capture_times[index] < capture_times[earliest_index])
{
@@ -94,7 +99,8 @@ bool gps_pps_select_reference(
}
}
*reference_index_out = earliest_index;
*reference_index_out =
(preferred_index != count) ? preferred_index : earliest_index;
return true;
}
soc/software/gps_system/src/gps/gps_time_anchor.c
+94
View File
@@ -212,6 +212,100 @@ bool gps_time_correct_capture_counter(
return true;
}
bool gps_nav_bit_phase_from_elapsed(
double anchor_tow_sec,
double elapsed_sec,
double bit_period_sec,
uint32_t nav_solve_ticks,
double guard_sec,
uint32_t* phase_seed_out)
{
if((phase_seed_out == NULL) || (nav_solve_ticks == 0u) ||
!isfinite(anchor_tow_sec) || !isfinite(elapsed_sec) ||
!isfinite(bit_period_sec) || !isfinite(guard_sec) ||
!(bit_period_sec > 0.0) || !(guard_sec >= 0.0) ||
(guard_sec >= (bit_period_sec / 2.0)) ||
(anchor_tow_sec < 0.0) ||
(anchor_tow_sec >= GPS_TIME_WEEK_SECONDS) ||
(elapsed_sec < 0.0))
{
return false;
}
double predicted_tow_sec = anchor_tow_sec + elapsed_sec;
double bit_index_exact = predicted_tow_sec / bit_period_sec;
double bit_index = floor(bit_index_exact);
if(!isfinite(bit_index) || (bit_index < 0.0))
{
return false;
}
/* The seed takes effect on the next navigation bit. Refuse to arm when
* that boundary is imminent, or when the current one has only just passed,
* so the applied phase cannot be one bit out. */
double into_bit_sec = (bit_index_exact - bit_index) * bit_period_sec;
if((into_bit_sec < guard_sec) ||
((bit_period_sec - into_bit_sec) < guard_sec))
{
return false;
}
double phase = fmod(bit_index + 1.0, (double)nav_solve_ticks);
if(!isfinite(phase) || (phase < 0.0) ||
(phase >= (double)nav_solve_ticks))
{
return false;
}
*phase_seed_out = (uint32_t)phase;
return true;
}
bool gps_nav_epoch_snap_from_elapsed(
double anchor_tow_sec,
double elapsed_sec,
double period_sec,
double max_snap_sec,
double* snapped_tow_sec_out)
{
if((snapped_tow_sec_out == NULL) ||
!isfinite(anchor_tow_sec) || !isfinite(elapsed_sec) ||
!isfinite(period_sec) || !isfinite(max_snap_sec) ||
!(period_sec > 0.0) || !(max_snap_sec >= 0.0) ||
(max_snap_sec >= (period_sec / 2.0)) ||
(anchor_tow_sec < 0.0) ||
(anchor_tow_sec >= GPS_TIME_WEEK_SECONDS) ||
(elapsed_sec < 0.0))
{
return false;
}
/*
* Snap the ELAPSED time, not the absolute time of week. Navigation-solve
* epochs are not multiples of the epoch period in time of week: the timer
* raises one on the navigation bit whose counter reaches N-1, so with N=10
* the epochs sit at 0.18 s modulo 0.2 s. What is exactly periodic is the
* spacing between epochs, and the anchor is itself an epoch, so the number
* of whole periods since it is the quantity to recover. Snapping the
* absolute time of week instead would introduce a fixed offset of up to
* half a period.
*/
double epoch_count = floor((elapsed_sec / period_sec) + 0.5);
double snapped_elapsed_sec = epoch_count * period_sec;
if(!isfinite(epoch_count) || (epoch_count < 0.0) ||
!isfinite(snapped_elapsed_sec) ||
(fabs(elapsed_sec - snapped_elapsed_sec) > max_snap_sec))
{
return false;
}
double snapped_tow_sec = anchor_tow_sec + snapped_elapsed_sec;
if(!isfinite(snapped_tow_sec))
{
return false;
}
*snapped_tow_sec_out = snapped_tow_sec;
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
+103 -4
View File
@@ -11,6 +11,15 @@ TARGETS += test_gps_satellite_velocity
TARGETS += test_gps_pps_time_filter
TARGETS += test_gps_pps_quality
TARGETS += test_gps_pps_adaptive_loop
TARGETS += test_gps_ephemeris_validity
TARGETS += test_gps_warm_reacquisition_sim
TARGETS += test_gps_ephemeris_store
TARGETS += test_gps_lnav_iode
TARGETS += test_gps_almanac
TARGETS += test_gps_nav_context_store
TARGETS += test_gps_nav_context_audit
TARGETS += test_gps_pvt_consistency
TARGETS += test_gps_measurement_model
.PHONY: all run sanitize clean
@@ -34,13 +43,66 @@ test_plic_warm_reset: test_plic_warm_reset.c $(ROOT)/../bsp/drivers/plic.c
test_gps_acq_manager: test_gps_acq_manager.c $(ROOT)/src/gps/gps_acq_manager.c
$(CC) $(CFLAGS) -I$(ROOT)/header $^ -o $@
test_gps_ephemeris_cache: test_gps_ephemeris_cache.c $(ROOT)/src/gps/gps_ephemeris_cache.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -o $@
test_gps_ephemeris_cache: test_gps_ephemeris_cache.c \
$(ROOT)/src/gps/gps_ephemeris_cache.c \
$(ROOT)/src/gps/gps_ephemeris_validity.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
test_gps_ephemeris_validity: test_gps_ephemeris_validity.c \
$(ROOT)/src/gps/gps_ephemeris_validity.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
test_gps_lnav_iode: test_gps_lnav_iode.c \
$(ROOT)/src/gps/gps_lnav_parser.c \
$(ROOT)/src/gps/gps_almanac.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
test_gps_almanac: test_gps_almanac.c \
$(ROOT)/src/gps/gps_almanac.c
$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@
test_gps_nav_context_store: test_gps_nav_context_store.c \
$(ROOT)/src/gps/gps_nav_context_store.c \
$(ROOT)/src/gps/gps_ephemeris_store.c \
$(ROOT)/src/gps/gps_ephemeris_validity.c \
$(ROOT)/src/gps/gps_lnav_parser.c \
$(ROOT)/src/gps/gps_almanac.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
test_gps_nav_context_audit: test_gps_nav_context_audit.c \
$(ROOT)/src/gps/gps_nav_context_store.c \
$(ROOT)/src/gps/gps_ephemeris_store.c \
$(ROOT)/src/gps/gps_ephemeris_cache.c \
$(ROOT)/src/gps/gps_ephemeris_validity.c \
$(ROOT)/src/gps/gps_lnav_parser.c \
$(ROOT)/src/gps/gps_almanac.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
test_gps_pvt_consistency: test_gps_pvt_consistency.c \
$(ROOT)/src/gps/gps_nav_epoch.c \
$(ROOT)/src/gps/gps_acq_manager.c \
$(ROOT)/src/gps/gps_ephemeris_cache.c \
$(ROOT)/src/gps/gps_ephemeris_validity.c \
$(ROOT)/src/gps/gps_pps_time_filter.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
test_gps_ephemeris_store: test_gps_ephemeris_store.c \
$(ROOT)/src/gps/gps_ephemeris_store.c \
$(ROOT)/src/gps/gps_ephemeris_validity.c \
$(ROOT)/src/gps/gps_lnav_parser.c \
$(ROOT)/src/gps/gps_almanac.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
test_gps_warm_reacquisition_sim: test_gps_warm_reacquisition_sim.c \
$(ROOT)/src/gps/gps_time_anchor.c \
$(ROOT)/src/gps/gps_nav_epoch.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
test_gps_receiver_hardware_sim: test_gps_receiver_hardware_sim.c \
$(ROOT)/src/gps/gps_acq_manager.c \
$(ROOT)/src/gps/gps_ephemeris_cache.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -o $@
$(ROOT)/src/gps/gps_ephemeris_cache.c \
$(ROOT)/src/gps/gps_ephemeris_validity.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
test_gps_tracking_measurement: test_gps_tracking_measurement.c \
$(ROOT)/src/gps/gps_tracking_measurement.c
@@ -53,6 +115,13 @@ test_gps_satellite_velocity: test_gps_satellite_velocity.c \
-Wno-unused-variable -Wno-sign-compare \
-Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
test_gps_measurement_model: test_gps_measurement_model.c \
$(ROOT)/src/gps/NavEquations.c \
$(ROOT)/src/gps/matrixLib.c
$(CC) $(CFLAGS) -Wno-comment -Wno-unused-parameter \
-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 $@
@@ -74,9 +143,18 @@ run: $(TARGETS)
./test_plic_warm_reset
./test_gps_acq_manager
./test_gps_ephemeris_cache
./test_gps_ephemeris_validity
./test_gps_warm_reacquisition_sim
./test_gps_ephemeris_store
./test_gps_lnav_iode
./test_gps_almanac
./test_gps_nav_context_store
./test_gps_nav_context_audit
./test_gps_pvt_consistency
./test_gps_receiver_hardware_sim
./test_gps_tracking_measurement
./test_gps_satellite_velocity
./test_gps_measurement_model
./test_gps_pps_time_filter
./test_gps_pps_quality
./test_gps_pps_adaptive_loop
@@ -115,15 +193,34 @@ sanitize:
-Wno-comment -Ifake_bsp -I$(ROOT)/header \
test_gps_ephemeris_cache.c \
$(ROOT)/src/gps/gps_ephemeris_cache.c \
$(ROOT)/src/gps/gps_ephemeris_validity.c -lm \
-o test_gps_ephemeris_cache_san
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 ./test_gps_ephemeris_cache_san
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
-fsanitize=address,undefined -fno-omit-frame-pointer \
-Wno-comment -Ifake_bsp -I$(ROOT)/header \
test_gps_ephemeris_validity.c \
$(ROOT)/src/gps/gps_ephemeris_validity.c -lm \
-o test_gps_ephemeris_validity_san
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 ./test_gps_ephemeris_validity_san
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
-fsanitize=address,undefined -fno-omit-frame-pointer \
-Wno-comment -Ifake_bsp -I$(ROOT)/header \
test_gps_warm_reacquisition_sim.c \
$(ROOT)/src/gps/gps_time_anchor.c \
$(ROOT)/src/gps/gps_nav_epoch.c -lm \
-o test_gps_warm_reacquisition_sim_san
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 ./test_gps_warm_reacquisition_sim_san
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
-fsanitize=address,undefined -fno-omit-frame-pointer \
-Wno-comment -Ifake_bsp -I$(ROOT)/header \
test_gps_receiver_hardware_sim.c \
$(ROOT)/src/gps/gps_acq_manager.c \
$(ROOT)/src/gps/gps_ephemeris_cache.c \
$(ROOT)/src/gps/gps_ephemeris_validity.c -lm \
-o test_gps_receiver_hardware_sim_san
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 \
@@ -177,6 +274,8 @@ clean:
test_gps_time_anchor_san \
test_gps_acq_manager_san test_gps_ephemeris_cache_san \
test_gps_receiver_hardware_sim_san \
test_gps_ephemeris_validity_san \
test_gps_warm_reacquisition_sim_san \
test_gps_tracking_measurement_san \
test_gps_satellite_velocity_san \
test_gps_pps_time_filter_san test_gps_pps_quality_san \
soc/software/gps_system/tests/test_gps_acq_manager.c
+191
View File
@@ -132,8 +132,199 @@ static void test_failed_hot_search_is_backed_off(void)
assert(start_next(&manager, 35u, true) == 20u);
}
/*
* Root cause found on the board: a satellite tracked seconds ago was served the
* 30 s hot backoff from its very first retry, while a never-seen cold PRN was
* retried every 0.5 s. The priority was inverted - the best candidate was
* penalised sixty times harder than the worst. Recency must buy fast retries
* that decay with failures, not be discarded immediately.
*/
static void a_freshly_lost_prn_is_retried_fast_before_backing_off(void)
{
gps_acq_manager_t manager;
const gps_acq_manager_config_t config = {
.retry_delay_ticks = 5u, /* fast cadence */
.hot_retry_delay_ticks = 300u, /* long backoff */
.hot_window_ticks = 7200u,
.max_hot_streak = 4u,
.hot_fast_attempts = 3u
};
gps_acq_manager_init(&manager, &config);
assert(gps_acq_manager_enable_cold(&manager, 7u));
uint64_t now = 1000u;
unsigned int prn = 0u;
bool hot = false;
/* Establish it as tracked, then lose it. */
assert(gps_acq_manager_start_next(&manager, now, &prn, &hot));
gps_acq_manager_mark_acquired(&manager, prn);
gps_acq_manager_mark_tracking(&manager, prn);
gps_acq_manager_mark_lost(&manager, 7u, now);
/* The first few retries must come at the fast cadence, not after 300. */
for(unsigned int attempt = 0u; attempt < config.hot_fast_attempts;
attempt++)
{
assert(gps_acq_manager_start_next(&manager, now, &prn, &hot));
assert(prn == 7u);
assert(hot);
gps_acq_manager_mark_failed(&manager, 7u, now);
now += config.retry_delay_ticks;
}
/* Allowance spent: it must now observe the long backoff. */
bool served_early = false;
for(unsigned int i = 0u; i < 5u; i++)
{
if(gps_acq_manager_start_next(&manager, now, &prn, &hot) &&
(prn == 7u) && hot)
{
served_early = true;
}
now += config.retry_delay_ticks;
}
assert(!served_early);
/* After the backoff it becomes a candidate again. */
now += config.hot_retry_delay_ticks;
bool served_late = false;
for(unsigned int i = 0u; i < 40u; i++)
{
if(gps_acq_manager_start_next(&manager, now, &prn, &hot) &&
(prn == 7u))
{
served_late = true;
break;
}
}
assert(served_late);
}
/*
* Almanac-directed acquisition. With one acquisition engine, sweeping PRNs
* that are below the horizon is what starves the satellites that are actually
* up: 75% of failed searches on the board were against below-horizon PRNs.
* The visibility prediction must bias the scheduler hard, yet remain purely
* advisory - it can never make a real satellite unreachable.
*/
static void test_invisible_prn_is_slowed_but_never_excluded(void)
{
const uint64_t retry = 10u;
const uint64_t unlikely = 500u;
gps_acq_manager_t manager;
const gps_acq_manager_config_t config = {
.retry_delay_ticks = retry,
.hot_retry_delay_ticks = 1000u,
.hot_window_ticks = 100u,
.max_hot_streak = 4u,
.hot_fast_attempts = 1u,
.unlikely_retry_delay_ticks = unlikely
};
gps_acq_manager_init(&manager, &config);
assert(gps_acq_manager_search_mask(&manager) == 0xffffffffu);
for(unsigned int prn = 0u; prn < GPS_ACQ_MANAGER_PRN_COUNT; prn++)
{
assert(gps_acq_manager_enable_cold(&manager, prn));
}
/* Only PRN index 3 is predicted visible. */
gps_acq_manager_set_search_mask(&manager, 1u << 3);
/*
* Early on, every search must go to the one visible PRN rather than round
* robin over the whole constellation.
*/
for(unsigned int attempt = 0u; attempt < 5u; attempt++)
{
uint64_t now = 1u + (attempt * retry);
unsigned int prn = GPS_ACQ_MANAGER_INVALID_PRN;
bool hot = true;
assert(gps_acq_manager_start_next(&manager, now, &prn, &hot));
assert(prn == 3u);
gps_acq_manager_mark_failed(&manager, prn, now);
}
/*
* But an invisible PRN must still come round eventually, so a stale
* almanac or a stale position self-corrects instead of blinding the
* receiver permanently.
*/
bool saw_invisible = false;
for(uint64_t now = unlikely; now <= (unlikely * 6u); now += retry)
{
unsigned int prn = GPS_ACQ_MANAGER_INVALID_PRN;
bool hot = true;
if(!gps_acq_manager_start_next(&manager, now, &prn, &hot))
{
continue;
}
gps_acq_manager_mark_failed(&manager, prn, now);
if(prn != 3u)
{
saw_invisible = true;
break;
}
}
assert(saw_invisible);
puts(" invisible PRNs are slowed but still revisited");
}
static void test_empty_mask_falls_back_to_searching_everything(void)
{
gps_acq_manager_t manager = make_manager(10u, 1000u, 100u, 4u);
for(unsigned int prn = 0u; prn < GPS_ACQ_MANAGER_PRN_COUNT; prn++)
{
assert(gps_acq_manager_enable_cold(&manager, prn));
}
/*
* A prediction that nothing is visible is always wrong, and honouring it
* would stall acquisition completely. Degrade to the blind sweep.
*/
gps_acq_manager_set_search_mask(&manager, 0u);
assert(gps_acq_manager_search_mask(&manager) == 0xffffffffu);
unsigned int prn = GPS_ACQ_MANAGER_INVALID_PRN;
bool hot = true;
assert(gps_acq_manager_start_next(&manager, 1u, &prn, &hot));
puts(" empty visibility mask falls back to searching everything");
}
static void test_visibility_does_not_disturb_a_visible_prn(void)
{
const uint64_t retry = 10u;
gps_acq_manager_t manager;
const gps_acq_manager_config_t config = {
.retry_delay_ticks = retry,
.hot_retry_delay_ticks = 1000u,
.hot_window_ticks = 100u,
.max_hot_streak = 4u,
.hot_fast_attempts = 4u,
.unlikely_retry_delay_ticks = 500u
};
gps_acq_manager_init(&manager, &config);
assert(gps_acq_manager_enable_cold(&manager, 5u));
gps_acq_manager_set_search_mask(&manager, 1u << 5);
/* A visible PRN keeps the fast cold cadence exactly as before. */
unsigned int prn = start_next(&manager, 1u, false);
assert(prn == 5u);
gps_acq_manager_mark_failed(&manager, prn, 1u);
prn = GPS_ACQ_MANAGER_INVALID_PRN;
bool hot = true;
assert(!gps_acq_manager_start_next(&manager, 1u + retry - 1u, &prn, &hot));
assert(gps_acq_manager_start_next(&manager, 1u + retry, &prn, &hot));
assert(prn == 5u);
puts(" visible PRNs keep the fast cadence");
}
int main(void)
{
test_invisible_prn_is_slowed_but_never_excluded();
test_empty_mask_falls_back_to_searching_everything();
test_visibility_does_not_disturb_a_visible_prn();
a_freshly_lost_prn_is_retried_fast_before_backing_off();
test_cold_round_robin_and_retry_from_tick_zero();
test_hot_priority_round_robin_and_cold_fairness();
test_expired_hot_becomes_cold_and_tracking_is_excluded();
soc/software/gps_system/tests/test_gps_almanac.c
+364
View File
@@ -0,0 +1,364 @@
/*
* Almanac decoder, orbital propagation and visibility-mask tests.
*
* The reference vector is a real broadcast almanac (PRN 1, GPS week 2431)
* quantised to the LNAV representation, with the expected ECEF position
* produced by an independent Python implementation of the IS-GPS-200
* user algorithm. That checks the bit layout and the propagation against
* something other than this code's own arithmetic.
*/
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "gps/gps_almanac.h"
/* -std=c11 does not expose TEST_PI. */
#define TEST_PI 3.1415926535897932
/*
* Place a field into a 30-bit LNAV word. first_bit is 1-based from the MSB
* of the word and the low six bits are parity, matching IS-GPS-200 and the
* decoder under test.
*/
static void put_field(unsigned int* word, unsigned int first_bit,
unsigned int length, uint32_t value)
{
unsigned int shift = 30u - (first_bit + length - 1u);
uint32_t mask = (length >= 32u) ? 0xffffffffu : ((1u << length) - 1u);
*word &= ~(mask << shift);
*word |= (value & mask) << shift;
}
static void build_almanac_page(unsigned int subframe[10],
unsigned char subframe_id,
unsigned char sv_id,
uint32_t ecc, uint32_t toa, int32_t delta_i,
int32_t omega_dot, uint32_t health,
uint32_t sqrt_a, int32_t omega0,
int32_t omega, int32_t m0,
int32_t af0, int32_t af1)
{
memset(subframe, 0, sizeof(unsigned int) * 10u);
/* Word 2 carries the subframe ID at bits 20..22. */
put_field(&subframe[1], 20u, 3u, subframe_id);
put_field(&subframe[2], 1u, 2u, 1u); /* data ID = 1 (LNAV) */
put_field(&subframe[2], 3u, 6u, sv_id);
put_field(&subframe[2], 9u, 16u, ecc);
put_field(&subframe[3], 1u, 8u, toa);
put_field(&subframe[3], 9u, 16u, (uint32_t)delta_i);
put_field(&subframe[4], 1u, 16u, (uint32_t)omega_dot);
put_field(&subframe[4], 17u, 8u, health);
put_field(&subframe[5], 1u, 24u, sqrt_a);
put_field(&subframe[6], 1u, 24u, (uint32_t)omega0);
put_field(&subframe[7], 1u, 24u, (uint32_t)omega);
put_field(&subframe[8], 1u, 24u, (uint32_t)m0);
/* af0 is split: bits 1..8 are the MSBs, bits 20..22 the LSBs. */
put_field(&subframe[9], 1u, 8u, ((uint32_t)af0 >> 3) & 0xffu);
put_field(&subframe[9], 9u, 11u, (uint32_t)af1);
put_field(&subframe[9], 20u, 3u, (uint32_t)af0 & 0x7u);
}
/* Real broadcast almanac for PRN 1, quantised to the LNAV fields. */
#define REF_ECC 3902
#define REF_TOA 36
#define REF_DELTA_I 2439
#define REF_OMEGA_DOT (-691)
#define REF_SQRT_A 10554475
#define REF_OMEGA0 677285
#define REF_OMEGA 459593
#define REF_M0 301402
#define REF_WEEK 2431u
#define REF_TOW 550000.0
static void build_reference_page(unsigned int subframe[10], unsigned char sv_id,
uint32_t health)
{
build_almanac_page(subframe, 5u, sv_id, REF_ECC, REF_TOA, REF_DELTA_I,
REF_OMEGA_DOT, health, REF_SQRT_A, REF_OMEGA0,
REF_OMEGA, REF_M0, -5, 3);
}
static void test_field_decoding(void)
{
unsigned int subframe[10];
gps_almanac_t almanac;
gps_almanac_reset(&almanac);
build_reference_page(subframe, 1u, 0u);
assert(gps_almanac_store_page(&almanac, subframe, 5u));
const gps_almanac_entry_t* e = &almanac.sv[0];
assert(e->valid);
assert(e->prn == 1u);
assert(e->health == 0u);
assert(fabs(e->ecc - (REF_ECC * ldexp(1.0, -21))) < 1e-15);
assert(fabs(e->toa_sec - (REF_TOA * 4096.0)) < 1e-9);
assert(fabs(e->sqrt_a - (REF_SQRT_A * ldexp(1.0, -11))) < 1e-9);
/* Negative OMEGADOT must survive sign extension of a 16-bit field. */
assert(e->omega_dot_rps < 0.0);
assert(fabs(e->omega_dot_rps -
(REF_OMEGA_DOT * ldexp(1.0, -38) * TEST_PI)) < 1e-18);
/* Inclination is an offset from 0.30 semicircles, not an absolute value. */
assert(fabs(e->i0_rad -
((0.30 + (REF_DELTA_I * ldexp(1.0, -19))) * TEST_PI)) < 1e-12);
/* af0 is reassembled from two separated bit groups. */
assert(fabs(e->af0_sec - (-5.0 * ldexp(1.0, -20))) < 1e-18);
assert(fabs(e->af1_sec_per_sec - (3.0 * ldexp(1.0, -38))) < 1e-24);
assert(gps_almanac_valid_count(&almanac) == 1u);
printf(" field decoding ok\n");
}
static void test_propagation_against_reference(void)
{
unsigned int subframe[10];
gps_almanac_t almanac;
gps_almanac_reset(&almanac);
build_reference_page(subframe, 1u, 0u);
assert(gps_almanac_store_page(&almanac, subframe, 5u));
double ecef[3];
assert(gps_almanac_satellite_ecef(&almanac.sv[0], REF_WEEK, REF_WEEK,
REF_TOW, ecef));
/*
* Independent Python implementation of the same user algorithm, with tk as
* the actual elapsed time since toa - here 402544 s, or 4.66 days.
*
* This vector replaces an earlier one generated with tk wrapped into the
* half week. That wrap was a misreading of IS-GPS-200 20.3.3.5.2.3, which
* is a week-crossover correction rather than a general reduction, and it
* put this satellite 5161 km from where it actually is - evaluating the
* orbit 2.3 days *before* toa instead of 4.7 days after it.
*/
const double expected[3] = {19464181.4246, 11780273.3193, 13764877.9908};
for(unsigned int i = 0u; i < 3u; i++)
{
double error = fabs(ecef[i] - expected[i]);
if(error > 1.0)
{
printf(" axis %u: got %.4f expected %.4f (error %.4f m)\n",
i, ecef[i], expected[i], error);
}
assert(error <= 1.0);
}
double radius = sqrt((ecef[0] * ecef[0]) + (ecef[1] * ecef[1]) +
(ecef[2] * ecef[2]));
assert(fabs(radius - 26591.4e3) < 5.0e3);
printf(" propagation matches reference within 1 m\n");
}
static void test_week_rollover_is_symmetric(void)
{
unsigned int subframe[10];
gps_almanac_t almanac;
gps_almanac_reset(&almanac);
build_reference_page(subframe, 1u, 0u);
assert(gps_almanac_store_page(&almanac, subframe, 5u));
/*
* The same instant expressed as (week, tow) or (week+1, tow-604800) must
* propagate to the same place, otherwise a week boundary would step the
* predicted satellite position and briefly corrupt the visible set.
*/
double a[3];
double b[3];
assert(gps_almanac_satellite_ecef(&almanac.sv[0], REF_WEEK, REF_WEEK,
600000.0, a));
assert(gps_almanac_satellite_ecef(&almanac.sv[0], REF_WEEK, REF_WEEK + 1u,
600000.0 - 604800.0, b));
for(unsigned int i = 0u; i < 3u; i++)
{
assert(fabs(a[i] - b[i]) < 1.0);
}
printf(" week rollover consistent\n");
}
static void test_page_rejection(void)
{
unsigned int subframe[10];
gps_almanac_t almanac;
gps_almanac_reset(&almanac);
/* SV ID 1..24 belongs to subframe 5; the same page on subframe 4 is not
* an almanac page and must not be stored. */
build_reference_page(subframe, 1u, 0u);
assert(!gps_almanac_store_page(&almanac, subframe, 4u));
/* SV ID 56 on subframe 4 is the ionosphere page, not an almanac. */
build_almanac_page(subframe, 4u, 56u, REF_ECC, REF_TOA, REF_DELTA_I,
REF_OMEGA_DOT, 0u, REF_SQRT_A, REF_OMEGA0,
REF_OMEGA, REF_M0, 0, 0);
assert(!gps_almanac_store_page(&almanac, subframe, 4u));
/* An all-zero (never uploaded) page has sqrt_a = 0 and must be rejected
* rather than stored as an entry that divides by zero when propagated. */
build_almanac_page(subframe, 5u, 7u, 0u, 0u, 0, 0, 0u, 0u, 0, 0, 0, 0, 0);
assert(!gps_almanac_store_page(&almanac, subframe, 5u));
assert(gps_almanac_valid_count(&almanac) == 0u);
printf(" page rejection ok\n");
}
/* Receiver position used for the visibility tests (Bolu, Turkey). */
static void receiver_ecef(double out[3])
{
const double lat = 40.735696 * TEST_PI / 180.0;
const double lon = 31.598649 * TEST_PI / 180.0;
const double h = 803.0;
const double a = 6378137.0;
const double f = 1.0 / 298.257223563;
const double e2 = f * (2.0 - f);
double n = a / sqrt(1.0 - (e2 * sin(lat) * sin(lat)));
out[0] = (n + h) * cos(lat) * cos(lon);
out[1] = (n + h) * cos(lat) * sin(lon);
out[2] = ((n * (1.0 - e2)) + h) * sin(lat);
}
static void test_visibility_fallbacks(void)
{
gps_almanac_t almanac;
uint32_t mask = 0u;
double rx[3];
receiver_ecef(rx);
/* No almanac at all: everything stays searchable. */
gps_almanac_reset(&almanac);
assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
&mask) == 0u);
assert(mask == 0xffffffffu);
/*
* Almanac entries but no subframe 5 page 25 yet: visibility must still be
* usable, falling back to the current week. Page 25 comes round only
* once per 12.5 minutes and waiting for it would leave the receiver
* sweeping blindly for that whole window after a reset.
*/
unsigned int subframe[10];
build_reference_page(subframe, 1u, 0u);
assert(gps_almanac_store_page(&almanac, subframe, 5u));
assert(!almanac.reference_valid);
mask = 0u;
assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
&mask) == 1u);
assert((mask & 1u) != 0u);
/*
* And that fallback must agree with the explicit reference when the
* reference does arrive, for an almanac of the current week.
*/
uint32_t mask_with_reference = 0u;
gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
&mask_with_reference) == 1u);
assert(mask_with_reference == mask);
gps_almanac_reset(&almanac);
assert(gps_almanac_store_page(&almanac, subframe, 5u));
/* No receiver position: all searchable. */
gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
mask = 0u;
assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, NULL, 0.0,
&mask) == 0u);
assert(mask == 0xffffffffu);
printf(" visibility fallbacks ok\n");
}
static void test_visibility_selects_and_never_hides_unknowns(void)
{
gps_almanac_t almanac;
unsigned int subframe[10];
uint32_t mask = 0u;
double rx[3];
receiver_ecef(rx);
gps_almanac_reset(&almanac);
gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
build_reference_page(subframe, 1u, 0u);
assert(gps_almanac_store_page(&almanac, subframe, 5u));
/* PRN 1 is above the horizon for this place and time (verified against
* the same almanac independently), so bit 0 must be set. */
double elevation = 0.0;
assert(gps_almanac_look_angles(&almanac.sv[0], REF_WEEK, REF_WEEK,
REF_TOW, rx, &elevation, NULL));
assert(elevation > 0.0);
assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
&mask) >= 1u);
assert((mask & 1u) != 0u);
/* Every PRN with no almanac entry yet must stay searchable, or a
* partially collected almanac would hide satellites we simply have not
* heard about. */
for(unsigned int prn_index = 1u; prn_index < 32u; prn_index++)
{
assert((mask & (1u << prn_index)) != 0u);
}
/* An unhealthy satellite is dropped from the visible set even though it
* is geometrically up. */
gps_almanac_reset(&almanac);
gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
build_reference_page(subframe, 1u, 0x3fu);
assert(gps_almanac_store_page(&almanac, subframe, 5u));
mask = 0xffffffffu;
assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
&mask) == 0u);
assert((mask & 1u) == 0u);
printf(" visibility selection ok\n");
}
static void test_satellite_sets_below_and_rises(void)
{
gps_almanac_t almanac;
unsigned int subframe[10];
double rx[3];
receiver_ecef(rx);
gps_almanac_reset(&almanac);
gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
build_reference_page(subframe, 1u, 0u);
assert(gps_almanac_store_page(&almanac, subframe, 5u));
/*
* Sweep a full orbit. A real GPS satellite must spend a substantial part
* of the day both above and below the horizon; a propagation sign error
* or a bad inclination typically pins it to one side.
*/
unsigned int above = 0u;
unsigned int below = 0u;
for(unsigned int step = 0u; step < 720u; step++)
{
double tow = 100000.0 + (step * 60.0);
double elevation = 0.0;
assert(gps_almanac_look_angles(&almanac.sv[0], REF_WEEK, REF_WEEK,
tow, rx, &elevation, NULL));
assert(elevation >= -90.0 && elevation <= 90.0);
if(elevation > 0.0)
{
above++;
}
else
{
below++;
}
}
assert(above > 60u);
assert(below > 60u);
printf(" rise/set over 12 h: %u above, %u below\n", above, below);
}
int main(void)
{
test_field_decoding();
test_propagation_against_reference();
test_week_rollover_is_symmetric();
test_page_rejection();
test_visibility_fallbacks();
test_visibility_selects_and_never_hides_unknowns();
test_satellite_sets_below_and_rises();
printf("gps almanac tests passed\n");
return 0;
}
soc/software/gps_system/tests/test_gps_ephemeris_cache.c
+83
View File
@@ -12,9 +12,16 @@ static gps_ephemeris_t make_ephemeris(
memset(&ephemeris, 0, sizeof(ephemeris));
ephemeris.SVID = (unsigned char)prn;
ephemeris.iode = iode;
ephemeris.iodc = iode;
ephemeris.ephemeris_valid = true;
ephemeris.sva = 2u;
ephemeris.sqrta = 5153.6;
ephemeris.A = ephemeris.sqrta * ephemeris.sqrta;
ephemeris.ecc = 0.008;
ephemeris.toe.week = 2430;
ephemeris.toe_week_valid = true;
ephemeris.toe.sec = 123456.0;
ephemeris.toc.sec = 123456.0;
return ephemeris;
}
@@ -72,10 +79,86 @@ static void test_validation_independence_and_invalidate(void)
&cache, 19u, 40u, &output));
}
static void test_store_if_new_reports_real_changes_only(void)
{
gps_ephemeris_cache_t cache;
gps_ephemeris_cache_init(&cache, 1000u);
gps_ephemeris_t eph = make_ephemeris(9u, 30u);
bool changed = true;
assert(gps_ephemeris_cache_store_if_new(&cache, 9u, &eph, 10u,
&changed));
assert(changed);
/* Re-decoding the same data set refreshes the age datum but reports no
* change, so no flash write is triggered. */
assert(gps_ephemeris_cache_store_if_new(&cache, 9u, &eph, 20u,
&changed));
assert(!changed);
gps_ephemeris_t output;
assert(gps_ephemeris_cache_restore(&cache, 9u, 1019u, &output));
assert(output.iode == 30u);
/* A newer upload is a real change. */
gps_ephemeris_t newer = make_ephemeris(9u, 31u);
newer.toe.sec = eph.toe.sec + 7200.0;
assert(gps_ephemeris_cache_store_if_new(&cache, 9u, &newer, 30u,
&changed));
assert(changed);
assert(gps_ephemeris_cache_restore(&cache, 9u, 30u, &output));
assert(output.iode == 31u);
/* An out-of-order older upload is rejected outright. */
assert(!gps_ephemeris_cache_store_if_new(&cache, 9u, &eph, 40u,
&changed));
assert(!changed);
assert(gps_ephemeris_cache_restore(&cache, 9u, 40u, &output));
assert(output.iode == 31u);
/* A structurally invalid record never enters the cache. */
gps_ephemeris_t corrupt = make_ephemeris(10u, 5u);
corrupt.iodc = 6u;
assert(!gps_ephemeris_cache_store_if_new(&cache, 10u, &corrupt, 50u,
&changed));
assert(!changed);
}
static void test_restore_at_applies_fit_window(void)
{
gps_ephemeris_cache_t cache;
gps_ephemeris_cache_init(&cache, 1000000u);
gps_ephemeris_t eph = make_ephemeris(12u, 44u);
gps_ephemeris_t output;
bool changed = false;
assert(gps_ephemeris_cache_store_if_new(&cache, 12u, &eph, 0u,
&changed));
/* Inside the four hour curve fit. */
assert(gps_ephemeris_cache_restore_at(&cache, 12u, 10u, true, 2430,
eph.toe.sec + 3600.0, &output));
assert(output.iode == 44u);
/* Without receiver time the record stays available as a candidate. */
assert(gps_ephemeris_cache_restore_at(&cache, 12u, 10u, false, 0, 0.0,
&output));
/* Past the fit interval the entry is rejected and dropped, so a later
* in-window query cannot resurrect it. */
assert(!gps_ephemeris_cache_restore_at(&cache, 12u, 10u, true, 2430,
eph.toe.sec + 9000.0, &output));
assert(!gps_ephemeris_cache_restore_at(&cache, 12u, 10u, true, 2430,
eph.toe.sec, &output));
assert(!gps_ephemeris_cache_restore(&cache, 12u, 10u, &output));
}
int main(void)
{
test_store_restore_and_expiry();
test_validation_independence_and_invalidate();
test_store_if_new_reports_real_changes_only();
test_restore_at_applies_fit_window();
puts("gps ephemeris cache tests passed");
return 0;
}
soc/software/gps_system/tests/test_gps_ephemeris_store.c
+663
View File
@@ -0,0 +1,663 @@
/*
* Persistent ephemeris store against a simulated SPI NOR device.
*
* The simulator enforces the properties that make flash different from RAM:
* programming can only clear bits, a program to a non-erased slot corrupts it,
* and a write can be cut short at any byte to model power loss.
*/
#include <assert.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "gps/gps_ephemeris_store.h"
#include "gps/gps_ephemeris_validity.h"
#define SECTOR_SIZE 4096u
#define SECTORS 2u
#define FLASH_SIZE (SECTOR_SIZE * SECTORS)
static int failures;
#define CHECK(condition, message) do { \
if(!(condition)) { \
fprintf(stderr, "FAIL: %s\n", message); \
failures++; \
} \
} while(0)
typedef struct {
uint8_t data[FLASH_SIZE];
uint32_t erase_count;
uint32_t program_count;
/* Power-loss injection: when armed, the Nth program truncates. */
int32_t fail_after_programs;
uint32_t truncate_bytes;
bool powered;
} sim_flash_t;
static void sim_init(sim_flash_t* f)
{
memset(f, 0, sizeof(*f));
memset(f->data, 0xff, sizeof(f->data));
f->fail_after_programs = -1;
f->powered = true;
}
static void sim_read(void* ctx, uint32_t address, void* dst, uint32_t len)
{
sim_flash_t* f = (sim_flash_t*)ctx;
assert((address + len) <= FLASH_SIZE);
memcpy(dst, &f->data[address], len);
}
static bool sim_erase(void* ctx, uint32_t address)
{
sim_flash_t* f = (sim_flash_t*)ctx;
if(!f->powered)
{
return false;
}
assert((address % SECTOR_SIZE) == 0u);
assert((address + SECTOR_SIZE) <= FLASH_SIZE);
memset(&f->data[address], 0xff, SECTOR_SIZE);
f->erase_count++;
return true;
}
static bool sim_program(void* ctx, uint32_t address, const void* src,
uint32_t len)
{
sim_flash_t* f = (sim_flash_t*)ctx;
if(!f->powered)
{
return false;
}
const uint8_t* bytes = (const uint8_t*)src;
assert((address + len) <= FLASH_SIZE);
uint32_t effective = len;
bool cut = false;
if(f->fail_after_programs == 0)
{
effective = (f->truncate_bytes < len) ? f->truncate_bytes : len;
cut = true;
}
if(f->fail_after_programs >= 0)
{
f->fail_after_programs--;
}
for(uint32_t i = 0u; i < effective; i++)
{
/* NOR programming can only clear bits. */
f->data[address + i] &= bytes[i];
}
f->program_count++;
if(cut)
{
f->powered = false;
return false;
}
return true;
}
static gps_ephemeris_store_flash_t sim_interface(sim_flash_t* f)
{
gps_ephemeris_store_flash_t iface;
memset(&iface, 0, sizeof(iface));
iface.read = sim_read;
iface.erase_sector = sim_erase;
iface.program = sim_program;
iface.ctx = f;
iface.base_address = 0u;
iface.sector_size = SECTOR_SIZE;
return iface;
}
/* ------------------------------------------------------------------ */
/* LNAV subframe construction */
/* ------------------------------------------------------------------ */
/*
* Build the minimum subframe content the decoder needs: the subframe id in the
* HOW, plus the fields the validity rules read back. The store re-parses these
* with the real decoder, so anything malformed here shows up as a rejected
* record rather than a silently wrong ephemeris.
*/
static void build_subframes(unsigned int words[3][10],
unsigned int iodc,
unsigned int toe_sec,
unsigned int health)
{
memset(words, 0, sizeof(unsigned int) * 3u * 10u);
unsigned int iode = iodc & 0xffu;
for(unsigned int sf = 0u; sf < 3u; sf++)
{
/* HOW: subframe id at bits 10..8, TOW count above bit 13. */
words[sf][1] = ((100u + sf) << 13) | ((sf + 1u) << 8);
}
/* Subframe 1: week, URA, health, IODC, Toc. */
words[0][2] = (380u << 20) | (0u << 18) | (2u << 14) |
((health & 0x3fu) << 8) | (((iodc >> 8) & 0x03u) << 6);
words[0][7] = ((iodc & 0xffu) << 22) | ((toe_sec >> 4) << 6);
/* Subframe 2: IODE, sqrtA, eccentricity, Toe, fit flag clear. */
words[1][2] = (iode << 22);
/* ecc = 0.008 -> 0.008 / 2^-33 */
unsigned int ecc_raw = (unsigned int)(0.008 * 8589934592.0);
words[1][5] = ((ecc_raw >> 24) & 0xffu) << 6;
words[1][6] = (ecc_raw & 0xffffffu) << 6;
/* sqrtA = 5153.6 -> / 2^-19 */
unsigned int sqrta_raw = (unsigned int)(5153.6 * 524288.0);
words[1][7] = ((sqrta_raw >> 24) & 0xffu) << 6;
words[1][8] = (sqrta_raw & 0xffffffu) << 6;
words[1][9] = ((toe_sec >> 4) << 14);
/* Subframe 3: IODE must match subframe 2. */
words[2][9] = (iode << 22);
}
typedef struct {
unsigned int count;
unsigned int prn[GPS_PRN_COUNT];
unsigned char iode[GPS_PRN_COUNT];
double toe[GPS_PRN_COUNT];
} load_result_t;
static void load_handler(void* ctx, unsigned int prn,
const gps_ephemeris_t* ephemeris)
{
load_result_t* result = (load_result_t*)ctx;
result->prn[result->count] = prn;
result->iode[result->count] = ephemeris->iode;
result->toe[result->count] = ephemeris->toe.sec;
result->count++;
}
static bool save_one(gps_ephemeris_store_t* store, unsigned int prn,
unsigned int iodc, unsigned int toe_sec,
bool* wrote_out)
{
unsigned int words[3][10];
build_subframes(words, iodc, toe_sec, 0u);
/* Parse with the live decoder so the stored record and the in-RAM
* ephemeris always agree by construction. */
gps_frame_t frame;
memset(&frame, 0, sizeof(frame));
frame.gps_ephems.SVID = (unsigned char)prn;
for(unsigned int sf = 0u; sf < 3u; sf++)
{
unsigned int how = 0u;
(void)parse_subframe(words[sf], &frame, &how);
}
frame.gps_ephems.SVID = (unsigned char)prn;
if(!frame.gps_ephems.ephemeris_valid)
{
return false;
}
return gps_ephemeris_store_save(store, prn, &frame.gps_ephems,
(const unsigned int (*)[10])words,
wrote_out);
}
/* ------------------------------------------------------------------ */
static void empty_flash_mounts_and_first_save_formats(void)
{
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface),
"a blank device failed to mount");
load_result_t result;
memset(&result, 0, sizeof(result));
CHECK(gps_ephemeris_store_load_all(&store, load_handler, &result) == 0u,
"a blank device returned records");
bool wrote = false;
CHECK(save_one(&store, 7u, 40u, 172800u, &wrote),
"the first save failed");
CHECK(wrote, "the first save did not write");
CHECK(flash.erase_count == 2u,
"formatting did not erase both sectors exactly once");
memset(&result, 0, sizeof(result));
CHECK(gps_ephemeris_store_load_all(&store, load_handler, &result) == 1u,
"the saved record was not returned");
CHECK((result.prn[0] == 7u) && (result.iode[0] == 40u),
"the restored record has the wrong identity");
CHECK(result.toe[0] == 172800.0, "the restored Toe is wrong");
}
static void records_survive_remount(void)
{
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
for(unsigned int prn = 1u; prn <= 8u; prn++)
{
bool wrote = false;
CHECK(save_one(&store, prn, 40u + prn, 172800u, &wrote),
"save failed while filling the store");
}
/* Cold boot. */
gps_ephemeris_store_t remounted;
CHECK(gps_ephemeris_store_mount(&remounted, &iface),
"remount after writes failed");
load_result_t result;
memset(&result, 0, sizeof(result));
unsigned int loaded =
gps_ephemeris_store_load_all(&remounted, load_handler, &result);
CHECK(loaded == 8u, "not every persisted PRN came back after remount");
for(unsigned int i = 0u; i < result.count; i++)
{
CHECK(result.iode[i] == (unsigned char)(40u + result.prn[i]),
"a restored record was attributed to the wrong PRN");
}
}
static void newer_upload_supersedes_and_old_one_is_ignored(void)
{
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
bool wrote = false;
CHECK(save_one(&store, 3u, 40u, 172800u, &wrote), "first save failed");
CHECK(save_one(&store, 3u, 41u, 180000u, &wrote), "second save failed");
gps_ephemeris_store_t remounted;
CHECK(gps_ephemeris_store_mount(&remounted, &iface), "remount failed");
load_result_t result;
memset(&result, 0, sizeof(result));
CHECK(gps_ephemeris_store_load_all(&remounted, load_handler, &result) == 1u,
"a superseded PRN returned more than one record");
CHECK(result.iode[0] == 41u,
"the older upload won over the newer one after remount");
CHECK(result.toe[0] == 180000.0, "the restored Toe is not the newest");
}
static void compaction_reclaims_a_full_sector(void)
{
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
const uint32_t slots = SECTOR_SIZE / GPS_EPH_STORE_SLOT_SIZE;
/* Rewrite one PRN until the sector must roll over, and keep a second PRN
* that must survive the compaction. */
bool wrote = false;
CHECK(save_one(&store, 9u, 60u, 100800u, &wrote), "seed save failed");
uint32_t toe = 172800u;
for(uint32_t i = 0u; i < (slots * 2u); i++)
{
CHECK(save_one(&store, 4u, 40u + (i % 200u), toe, &wrote),
"save failed during compaction stress");
toe += 16u;
}
CHECK(store.compactions > 0u, "the store never compacted");
CHECK(flash.erase_count <= (2u + (store.compactions * 2u)),
"compaction erased more sectors than it should");
gps_ephemeris_store_t remounted;
CHECK(gps_ephemeris_store_mount(&remounted, &iface),
"remount after compaction failed");
load_result_t result;
memset(&result, 0, sizeof(result));
unsigned int loaded =
gps_ephemeris_store_load_all(&remounted, load_handler, &result);
CHECK(loaded == 2u, "compaction lost a live PRN");
bool saw_9 = false;
for(unsigned int i = 0u; i < result.count; i++)
{
if(result.prn[i] == 9u)
{
saw_9 = true;
CHECK(result.iode[i] == 60u,
"the carried-over PRN changed during compaction");
}
}
CHECK(saw_9, "the untouched PRN did not survive compaction");
}
static void a_torn_record_is_skipped_and_the_store_still_mounts(void)
{
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
bool wrote = false;
CHECK(save_one(&store, 5u, 40u, 172800u, &wrote), "first save failed");
CHECK(save_one(&store, 6u, 50u, 172800u, &wrote), "second save failed");
/* Cut the next record halfway through: magic lands, CRC does not. */
flash.fail_after_programs = 0;
flash.truncate_bytes = 32u;
(void)save_one(&store, 5u, 41u, 180000u, &wrote);
flash.powered = true;
flash.fail_after_programs = -1;
gps_ephemeris_store_t remounted;
CHECK(gps_ephemeris_store_mount(&remounted, &iface),
"the store did not mount after a torn write");
CHECK(remounted.records_rejected > 0u,
"the torn record was not detected");
load_result_t result;
memset(&result, 0, sizeof(result));
unsigned int loaded =
gps_ephemeris_store_load_all(&remounted, load_handler, &result);
CHECK(loaded == 2u, "a torn write destroyed good records");
for(unsigned int i = 0u; i < result.count; i++)
{
if(result.prn[i] == 5u)
{
CHECK(result.iode[i] == 40u,
"the torn record was accepted over the good one");
}
}
}
static void power_loss_during_compaction_leaves_a_mountable_store(void)
{
const uint32_t slots = SECTOR_SIZE / GPS_EPH_STORE_SLOT_SIZE;
/* Cut power at each program of the compaction sequence in turn. */
for(int32_t cut = 0; cut < 6; cut++)
{
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
bool wrote = false;
CHECK(save_one(&store, 2u, 30u, 100800u, &wrote), "seed save failed");
for(uint32_t i = 0u; i + 2u < slots; i++)
{
CHECK(save_one(&store, 8u, 40u + (i % 200u), 172800u + (i * 16u),
&wrote),
"fill save failed");
}
/* The next save triggers compaction; cut power inside it. */
flash.fail_after_programs = cut;
flash.truncate_bytes = 16u;
(void)save_one(&store, 8u, 99u, 200000u, &wrote);
/* Power restored: the fault must not still be armed, or the recovery
* write is measuring the injector rather than the store. */
flash.powered = true;
flash.fail_after_programs = -1;
gps_ephemeris_store_t remounted;
CHECK(gps_ephemeris_store_mount(&remounted, &iface),
"the store did not mount after power loss in compaction");
load_result_t result;
memset(&result, 0, sizeof(result));
(void)gps_ephemeris_store_load_all(&remounted, load_handler, &result);
/* The store may legitimately have lost the in-flight record, but it
* must never return a corrupt one, and it must stay writable. */
for(unsigned int i = 0u; i < result.count; i++)
{
CHECK(result.prn[i] < GPS_PRN_COUNT,
"a corrupt PRN survived power loss");
}
bool wrote_after = false;
CHECK(save_one(&remounted, 11u, 70u, 172800u, &wrote_after),
"the store was not writable after power loss");
}
}
static void a_corrupt_header_falls_back_to_the_other_sector(void)
{
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
bool wrote = false;
CHECK(save_one(&store, 12u, 44u, 172800u, &wrote), "save failed");
/* Corrupt the active sector header only. */
flash.data[4] ^= 0xffu;
gps_ephemeris_store_t remounted;
CHECK(gps_ephemeris_store_mount(&remounted, &iface),
"mount failed with a corrupt header");
load_result_t result;
memset(&result, 0, sizeof(result));
(void)gps_ephemeris_store_load_all(&remounted, load_handler, &result);
/* Sector 1 was erased at format and has no valid header either, so the
* store must come up empty rather than trusting garbage. */
CHECK(result.count == 0u,
"records were returned from a sector with no valid header");
CHECK(save_one(&remounted, 12u, 44u, 172800u, &wrote),
"the store did not recover after header corruption");
}
static void rewriting_the_same_ephemeris_is_the_callers_decision(void)
{
/*
* The store itself always appends what it is given; the wear policy lives
* in gps_ephemeris_supersedes, which the caller applies first. Verify the
* two agree so no repeat write can reach flash.
*/
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
unsigned int words[3][10];
build_subframes(words, 40u, 172800u, 0u);
gps_frame_t frame;
memset(&frame, 0, sizeof(frame));
frame.gps_ephems.SVID = 15u;
for(unsigned int sf = 0u; sf < 3u; sf++)
{
unsigned int how = 0u;
(void)parse_subframe(words[sf], &frame, &how);
}
frame.gps_ephems.SVID = 15u;
CHECK(!gps_ephemeris_supersedes(&frame.gps_ephems, &frame.gps_ephems),
"an identical re-decode was reported as new information");
bool wrote = false;
CHECK(gps_ephemeris_store_save(&store, 15u, &frame.gps_ephems,
(const unsigned int (*)[10])words, &wrote),
"save failed");
uint32_t writes_after_first = store.writes;
/* The caller would not call save again, but confirm the record is stable
* if it did: same content, one extra slot, same restored result. */
CHECK(gps_ephemeris_store_save(&store, 15u, &frame.gps_ephems,
(const unsigned int (*)[10])words, &wrote),
"second save failed");
CHECK(store.writes == (writes_after_first + 1u),
"an unexpected number of flash writes occurred");
gps_ephemeris_store_t remounted;
CHECK(gps_ephemeris_store_mount(&remounted, &iface), "remount failed");
load_result_t result;
memset(&result, 0, sizeof(result));
CHECK(gps_ephemeris_store_load_all(&remounted, load_handler, &result) == 1u,
"duplicate saves produced duplicate restores");
}
static void an_unhealthy_or_inconsistent_record_is_refused(void)
{
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
unsigned int words[3][10];
build_subframes(words, 40u, 172800u, 0u);
gps_frame_t frame;
memset(&frame, 0, sizeof(frame));
frame.gps_ephems.SVID = 20u;
for(unsigned int sf = 0u; sf < 3u; sf++)
{
unsigned int how = 0u;
(void)parse_subframe(words[sf], &frame, &how);
}
frame.gps_ephems.SVID = 20u;
gps_ephemeris_t unhealthy = frame.gps_ephems;
unhealthy.svhlth = 0x3fu;
bool wrote = true;
CHECK(!gps_ephemeris_store_save(&store, 20u, &unhealthy,
(const unsigned int (*)[10])words, &wrote),
"an unhealthy satellite was persisted");
CHECK(!wrote, "a refused save reported a write");
CHECK(!gps_ephemeris_store_save(&store, 21u, &frame.gps_ephems,
(const unsigned int (*)[10])words, &wrote),
"a record was persisted under the wrong PRN");
}
static void crc_detects_single_bit_corruption(void)
{
uint8_t buffer[64];
for(unsigned int i = 0u; i < sizeof(buffer); i++)
{
buffer[i] = (uint8_t)i;
}
uint32_t base = gps_ephemeris_store_crc32(buffer, sizeof(buffer));
for(unsigned int i = 0u; i < sizeof(buffer); i++)
{
for(unsigned int bit = 0u; bit < 8u; bit++)
{
buffer[i] ^= (uint8_t)(1u << bit);
CHECK(gps_ephemeris_store_crc32(buffer, sizeof(buffer)) != base,
"CRC missed a single-bit flip");
buffer[i] ^= (uint8_t)(1u << bit);
}
}
/* Known-answer check against the standard CRC-32 of "123456789". */
CHECK(gps_ephemeris_store_crc32("123456789", 9u) == 0xcbf43926u,
"CRC-32 does not match the reference value");
}
static void slot_cap_forces_compaction_on_a_real_sector(void)
{
/* The hardware test hook: cap slots so compaction is reachable without
* writing a full sector, while still exercising real erases. */
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
gps_ephemeris_store_limit_slots(&store, 4u);
CHECK(store.slots_per_sector == 4u, "slot cap was not applied");
bool wrote = false;
CHECK(save_one(&store, 5u, 30u, 100800u, &wrote), "seed save failed");
uint32_t toe = 172800u;
for(unsigned int i = 0u; i < 12u; i++)
{
CHECK(save_one(&store, 6u, 40u + i, toe, &wrote), "fill save failed");
toe += 16u;
}
CHECK(store.compactions > 0u, "slot cap did not reach compaction");
CHECK(flash.erase_count >= 3u, "compaction did not perform real erases");
gps_ephemeris_store_t remounted;
CHECK(gps_ephemeris_store_mount(&remounted, &iface), "remount failed");
gps_ephemeris_store_limit_slots(&remounted, 4u);
load_result_t result;
memset(&result, 0, sizeof(result));
unsigned int loaded =
gps_ephemeris_store_load_all(&remounted, load_handler, &result);
CHECK(loaded == 2u, "capped compaction lost a live PRN");
/* The cap only shrinks, and refuses degenerate values. */
gps_ephemeris_store_limit_slots(&remounted, 1u);
CHECK(remounted.slots_per_sector == 4u, "cap accepted a degenerate value");
gps_ephemeris_store_limit_slots(&remounted, 999u);
CHECK(remounted.slots_per_sector == 4u, "cap grew the sector");
}
static void compaction_that_cannot_fit_latches_instead_of_eroding_flash(void)
{
/*
* Regression for a destructive failure mode found on hardware: compaction
* erases the spare sector before discovering the live set does not fit.
* Without a latch, every subsequent save retried and burned two more erase
* cycles, indefinitely.
*/
sim_flash_t flash;
sim_init(&flash);
gps_ephemeris_store_t store;
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
gps_ephemeris_store_limit_slots(&store, 4u); /* header + 3 records */
bool wrote = false;
/* Four distinct PRNs cannot be carried into three record slots. */
for(unsigned int prn = 1u; prn <= 4u; prn++)
{
(void)save_one(&store, prn, 40u + prn, 172800u, &wrote);
}
/* Force the sector full so the next save must compact. */
for(unsigned int i = 0u; i < 4u; i++)
{
(void)save_one(&store, 4u, 90u + i, 180000u + (i * 16u), &wrote);
}
CHECK(store.capacity_exceeded, "over-capacity compaction did not latch");
uint32_t erases_after_latch = flash.erase_count;
/* Every further save must be refused without touching the device. */
for(unsigned int i = 0u; i < 10u; i++)
{
CHECK(!save_one(&store, 2u, 50u + i, 190000u, &wrote),
"a save succeeded after the capacity latch");
CHECK(!wrote, "a latched store reported a write");
}
CHECK(flash.erase_count == erases_after_latch,
"the latched store kept erasing flash");
/* A remount clears the latch, so recovery is possible once the config or
* the live set changes. */
gps_ephemeris_store_t remounted;
CHECK(gps_ephemeris_store_mount(&remounted, &iface), "remount failed");
CHECK(!remounted.capacity_exceeded, "the latch survived a remount");
}
int main(void)
{
crc_detects_single_bit_corruption();
compaction_that_cannot_fit_latches_instead_of_eroding_flash();
slot_cap_forces_compaction_on_a_real_sector();
empty_flash_mounts_and_first_save_formats();
records_survive_remount();
newer_upload_supersedes_and_old_one_is_ignored();
compaction_reclaims_a_full_sector();
a_torn_record_is_skipped_and_the_store_still_mounts();
power_loss_during_compaction_leaves_a_mountable_store();
a_corrupt_header_falls_back_to_the_other_sector();
rewriting_the_same_ephemeris_is_the_callers_decision();
an_unhealthy_or_inconsistent_record_is_refused();
if(failures == 0)
{
puts("gps ephemeris store tests passed");
}
return failures == 0 ? 0 : 1;
}
soc/software/gps_system/tests/test_gps_ephemeris_validity.c
+253
View File
@@ -0,0 +1,253 @@
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "gps/gps_ephemeris_validity.h"
#define WEEK_SEC 604800.0
static gps_ephemeris_t make_ephemeris(unsigned int prn, unsigned int iodc)
{
gps_ephemeris_t eph;
memset(&eph, 0, sizeof(eph));
eph.SVID = (unsigned char)prn;
eph.ephemeris_valid = true;
eph.iodc = iodc;
eph.iode = (unsigned char)(iodc & 0xffu);
eph.sva = 2u;
eph.svhlth = 0u;
eph.update = 0u;
eph.sqrta = 5153.6;
eph.A = eph.sqrta * eph.sqrta;
eph.ecc = 0.008;
eph.toe.sec = 172800.0;
eph.toc.sec = 172800.0;
eph.WN = 430u;
eph.tow.week = 2430;
/* Resolved from WN and the handover word by gps_resolve_toe_week(). */
eph.toe.week = 2430;
eph.toe_week_valid = true;
return eph;
}
static void test_fit_interval_table(void)
{
/* Fit flag clear is always the nominal four hour curve fit. */
assert(gps_ephemeris_fit_interval_hours(0u, false) == 4.0);
assert(gps_ephemeris_fit_interval_hours(500u, false) == 4.0);
/* IS-GPS-200 Table 20-XII ranges. */
assert(gps_ephemeris_fit_interval_hours(240u, true) == 8.0);
assert(gps_ephemeris_fit_interval_hours(247u, true) == 8.0);
assert(gps_ephemeris_fit_interval_hours(248u, true) == 14.0);
assert(gps_ephemeris_fit_interval_hours(496u, true) == 14.0);
assert(gps_ephemeris_fit_interval_hours(497u, true) == 26.0);
assert(gps_ephemeris_fit_interval_hours(504u, true) == 50.0);
assert(gps_ephemeris_fit_interval_hours(511u, true) == 74.0);
/* Untabulated IODC with the flag set falls back conservatively, below
* the shortest long fit the ICD defines. */
double fallback = gps_ephemeris_fit_interval_hours(100u, true);
assert(fallback == GPS_EPHEMERIS_LONG_FIT_FALLBACK_HOURS);
assert(fallback < 8.0);
}
static void test_static_rules(void)
{
gps_ephemeris_t eph = make_ephemeris(7u, 40u);
assert(gps_ephemeris_check_static(&eph, 7u) == GPS_EPHEMERIS_OK);
assert(gps_ephemeris_check_static(NULL, 7u) ==
GPS_EPHEMERIS_REJECT_NULL);
assert(gps_ephemeris_check_static(&eph, 8u) ==
GPS_EPHEMERIS_REJECT_PRN);
gps_ephemeris_t uncommitted = eph;
uncommitted.ephemeris_valid = false;
assert(gps_ephemeris_check_static(&uncommitted, 7u) ==
GPS_EPHEMERIS_REJECT_UNCOMMITTED);
/* IODE must equal the eight LSBs of IODC. */
gps_ephemeris_t mismatched = eph;
mismatched.iode = (unsigned char)(mismatched.iode + 1u);
assert(gps_ephemeris_check_static(&mismatched, 7u) ==
GPS_EPHEMERIS_REJECT_IOD);
/* A ten bit IODC whose low byte matches is accepted. */
gps_ephemeris_t wide_iodc = make_ephemeris(7u, 0x123u);
assert(wide_iodc.iode == 0x23u);
assert(gps_ephemeris_check_static(&wide_iodc, 7u) == GPS_EPHEMERIS_OK);
gps_ephemeris_t unhealthy = eph;
unhealthy.svhlth = 0x20u;
assert(gps_ephemeris_check_static(&unhealthy, 7u) ==
GPS_EPHEMERIS_REJECT_HEALTH);
gps_ephemeris_t no_accuracy = eph;
no_accuracy.sva = 15u;
assert(gps_ephemeris_check_static(&no_accuracy, 7u) ==
GPS_EPHEMERIS_REJECT_ACCURACY);
gps_ephemeris_t bad_orbit = eph;
bad_orbit.ecc = 1.5;
assert(gps_ephemeris_check_static(&bad_orbit, 7u) ==
GPS_EPHEMERIS_REJECT_PARAMETERS);
gps_ephemeris_t bad_toe = eph;
bad_toe.toe.sec = WEEK_SEC;
assert(gps_ephemeris_check_static(&bad_toe, 7u) ==
GPS_EPHEMERIS_REJECT_PARAMETERS);
}
static void test_fit_window(void)
{
gps_ephemeris_t eph = make_ephemeris(7u, 40u);
const double toe = eph.toe.sec;
/* Nominal four hour fit: usable for two hours either side of Toe. */
assert(gps_ephemeris_max_toe_offset_sec(&eph) == 7200.0);
assert(gps_ephemeris_check(&eph, 7u, true, 2430, toe) ==
GPS_EPHEMERIS_OK);
assert(gps_ephemeris_check(&eph, 7u, true, 2430, toe + 7199.0) ==
GPS_EPHEMERIS_OK);
assert(gps_ephemeris_check(&eph, 7u, true, 2430, toe - 7199.0) ==
GPS_EPHEMERIS_OK);
assert(gps_ephemeris_check(&eph, 7u, true, 2430, toe + 7201.0) ==
GPS_EPHEMERIS_REJECT_AGE);
assert(gps_ephemeris_check(&eph, 7u, true, 2430, toe - 7201.0) ==
GPS_EPHEMERIS_REJECT_AGE);
/* An extended fit interval widens the same window. */
gps_ephemeris_t extended = make_ephemeris(7u, 250u);
extended.fit_flag = true;
assert(gps_ephemeris_max_toe_offset_sec(&extended) == 14.0 * 1800.0);
assert(gps_ephemeris_check(&extended, 7u, true, 2430, toe + 20000.0) ==
GPS_EPHEMERIS_OK);
/* Without receiver time only the static rules apply, so a boot-time
* flash record stays a usable candidate. */
assert(gps_ephemeris_check(&eph, 7u, false, 0, 0.0) ==
GPS_EPHEMERIS_OK);
}
static void test_week_rollover(void)
{
/* Toe 600 s before the end of the week, receiver time 600 s after the
* rollover: the true age is 1200 s, not almost a full week. */
gps_ephemeris_t eph = make_ephemeris(11u, 60u);
eph.toe.sec = WEEK_SEC - 600.0;
double offset = 0.0;
assert(gps_ephemeris_toe_offset_sec(&eph, 2431, 600.0, &offset));
assert(fabs(offset - 1200.0) < 1e-6);
assert(gps_ephemeris_check(&eph, 11u, true, 2431, 600.0) ==
GPS_EPHEMERIS_OK);
/* The same record is stale eight hours into the new week. */
assert(gps_ephemeris_check(&eph, 11u, true, 2431, 28800.0) ==
GPS_EPHEMERIS_REJECT_AGE);
/* Toe just after a rollover with receiver time just before it gives a
* small negative age rather than a week-long one. */
gps_ephemeris_t fresh = make_ephemeris(11u, 60u);
fresh.toe.sec = 600.0;
/* Toe belongs to the week after the one it was broadcast in. */
fresh.toe.week = 2431;
assert(gps_ephemeris_toe_offset_sec(&fresh, 2430, WEEK_SEC - 600.0,
&offset));
assert(fabs(offset + 1200.0) < 1e-6);
assert(gps_ephemeris_check(&fresh, 11u, true, 2430,
WEEK_SEC - 600.0) == GPS_EPHEMERIS_OK);
}
/*
* Toe is a second-of-week, so before its week was resolved the age check could
* only work modulo one week: a record exactly N weeks old, evaluated at a
* similar time of week, read as brand new. While ephemeris lives in RAM and
* expires in hours that is unreachable. Persisting it to flash makes it
* directly reachable - switch the receiver off and on again a week later at a
* similar hour and it would accept its own week-old orbits as current, which
* is worth kilometres of position error with nothing to flag it.
*/
static void test_whole_week_staleness_is_not_invisible(void)
{
gps_ephemeris_t eph = make_ephemeris(11u, 60u);
double offset = 0.0;
/* Same second of week, one week on. */
assert(gps_ephemeris_toe_offset_sec(&eph, 2431, eph.toe.sec, &offset));
assert(fabs(offset - WEEK_SEC) < 1e-6);
assert(gps_ephemeris_check(&eph, 11u, true, 2431, eph.toe.sec) ==
GPS_EPHEMERIS_REJECT_AGE);
/* And four weeks on, which is what a shelved receiver comes back to. */
assert(gps_ephemeris_check(&eph, 11u, true, 2434, eph.toe.sec) ==
GPS_EPHEMERIS_REJECT_AGE);
/* A record whose week could not be resolved has an unknown vintage and
* must be refused rather than assumed current. */
gps_ephemeris_t unresolved = make_ephemeris(11u, 60u);
unresolved.toe_week_valid = false;
assert(!gps_ephemeris_toe_offset_sec(&unresolved, 2430,
unresolved.toe.sec + 60.0, &offset));
assert(gps_ephemeris_check(&unresolved, 11u, true, 2430,
unresolved.toe.sec + 60.0) ==
GPS_EPHEMERIS_REJECT_AGE);
/* Without receiver time it is still a usable cold-start candidate; the
* epoch gate re-checks it the moment time exists. */
assert(gps_ephemeris_check(&unresolved, 11u, false, 0, 0.0) ==
GPS_EPHEMERIS_OK);
printf(" whole-week staleness is visible to the age check\n");
}
static void test_supersede(void)
{
gps_ephemeris_t current = make_ephemeris(19u, 40u);
gps_ephemeris_t repeat = current;
/* The same data set decoded again is not new information. */
assert(!gps_ephemeris_supersedes(&repeat, &current));
/* Health or transmitted accuracy can change inside one issue of data. */
gps_ephemeris_t health_change = current;
health_change.svhlth = 0x3fu;
assert(gps_ephemeris_supersedes(&health_change, &current));
/* A newer upload with a later Toe replaces it. */
gps_ephemeris_t newer = make_ephemeris(19u, 41u);
newer.toe.sec = current.toe.sec + 7200.0;
assert(gps_ephemeris_supersedes(&newer, &current));
/* An older Toe with a different IODE does not: that is a replayed or
* out-of-order decode. */
gps_ephemeris_t older = make_ephemeris(19u, 39u);
older.toe.sec = current.toe.sec - 7200.0;
assert(!gps_ephemeris_supersedes(&older, &current));
/* Toe advancing across the week boundary still counts as newer. */
gps_ephemeris_t before_rollover = make_ephemeris(19u, 42u);
before_rollover.toe.sec = WEEK_SEC - 1800.0;
gps_ephemeris_t after_rollover = make_ephemeris(19u, 43u);
after_rollover.toe.sec = 1800.0;
assert(gps_ephemeris_supersedes(&after_rollover, &before_rollover));
assert(!gps_ephemeris_supersedes(&before_rollover, &after_rollover));
/* Nothing supersedes across PRNs, and anything replaces an empty slot. */
gps_ephemeris_t other_prn = make_ephemeris(20u, 41u);
assert(!gps_ephemeris_supersedes(&other_prn, &current));
assert(gps_ephemeris_supersedes(&current, NULL));
assert(!gps_ephemeris_supersedes(NULL, &current));
}
int main(void)
{
test_fit_interval_table();
test_static_rules();
test_fit_window();
test_week_rollover();
test_whole_week_staleness_is_not_invisible();
test_supersede();
puts("gps ephemeris validity tests passed");
return 0;
}
soc/software/gps_system/tests/test_gps_lnav_iode.c
+168
View File
@@ -0,0 +1,168 @@
#include <stdio.h>
#include <string.h>
#include "gps/gps_ip_defines.h"
static int failures;
#define CHECK(c,m) do{ if(!(c)){ fprintf(stderr,"FAIL: %s\n",m); failures++; } }while(0)
int main(void)
{
unsigned int sf[10];
unsigned char iode = 0u;
/* Subframe 1: IODC is split, two MSBs in word 3 bits 7..6, eight LSBs in
* word 8 bits 29..22. The low byte must equal the IODE. */
memset(sf, 0, sizeof(sf));
sf[2] = (0x02u << 6); /* IODC[9:8] = 2 */
sf[7] = (0x5Au << 22); /* IODC[7:0] = 0x5A */
CHECK(gps_lnav_broadcast_iode(sf, 1u, &iode), "sf1 iode not extracted");
CHECK(iode == 0x5Au, "sf1 iode wrong");
/* Subframe 2: IODE in word 3 bits 29..22. */
memset(sf, 0, sizeof(sf));
sf[2] = (0xA3u << 22);
CHECK(gps_lnav_broadcast_iode(sf, 2u, &iode), "sf2 iode not extracted");
CHECK(iode == 0xA3u, "sf2 iode wrong");
/* Subframe 3: IODE in word 10 bits 29..22. */
memset(sf, 0, sizeof(sf));
sf[9] = (0x17u << 22);
CHECK(gps_lnav_broadcast_iode(sf, 3u, &iode), "sf3 iode not extracted");
CHECK(iode == 0x17u, "sf3 iode wrong");
/* Subframes 4 and 5 carry no issue of data. */
CHECK(!gps_lnav_broadcast_iode(sf, 4u, &iode), "sf4 reported an iode");
CHECK(!gps_lnav_broadcast_iode(sf, 5u, &iode), "sf5 reported an iode");
CHECK(!gps_lnav_broadcast_iode(NULL, 2u, &iode), "null subframe accepted");
CHECK(!gps_lnav_broadcast_iode(sf, 2u, NULL), "null output accepted");
/* All three subframes of one data set must agree, which is what the
* cross-check relies on. */
unsigned int set1[10], set2[10], set3[10];
memset(set1,0,sizeof(set1)); memset(set2,0,sizeof(set2)); memset(set3,0,sizeof(set3));
set1[2] = (0x01u << 6); set1[7] = (0x77u << 22);
set2[2] = (0x77u << 22);
set3[9] = (0x77u << 22);
unsigned char a=0,b=0,c=0;
gps_lnav_broadcast_iode(set1,1u,&a);
gps_lnav_broadcast_iode(set2,2u,&b);
gps_lnav_broadcast_iode(set3,3u,&c);
CHECK((a==b)&&(b==c)&&(a==0x77u), "one data set gave inconsistent iode");
/*
* Restoring a persisted page must repopulate the raw-word cache, not just
* the parsed model. The periodic save rebuilds its payload from that
* cache, so a restore that only set the parsed form would write the page
* back as absent and wipe the stored model on the first save after every
* reboot. That regression reached hardware once and was caught only by
* watching "iono=0" appear in the save telemetry.
*/
unsigned int page18[10];
memset(page18, 0, sizeof(page18));
/* data ID 1, SV ID 56 -> subframe 4 page 18, with non-zero coefficients. */
page18[2] = (1u << 28) | (56u << 22) | (0x1au << 14) | (0x0bu << 6);
page18[3] = (0x21u << 22) | (0x33u << 14) | (0x44u << 6);
page18[4] = (0x55u << 22) | (0x66u << 14) | (0x77u << 6);
page18[5] = (0x12u << 22) | (0x34u << 14) | (0x56u << 6);
uint32_t words[10];
CHECK(!gps_lnav_get_iono_page(words),
"iono page reported before anything was stored");
CHECK(gps_lnav_restore_iono_page(page18), "restoring page 18 failed");
CHECK(gps_lnav_get_iono_page(words),
"restored iono page did not repopulate the raw-word cache");
for(unsigned int i = 0u; i < 10u; i++)
{
CHECK(words[i] == (uint32_t)page18[i],
"restored iono raw words differ from what was restored");
}
/* Same contract for almanac pages. */
unsigned int alm[10];
memset(alm, 0, sizeof(alm));
alm[2] = (1u << 28) | (5u << 22) | (0x0f3eu << 6); /* SV ID 5 */
alm[3] = (36u << 22) | (2439u << 6);
alm[4] = (0xfd50u << 14);
alm[5] = (10554475u << 6);
uint32_t pages[GPS_ALMANAC_PRN_COUNT][10];
CHECK(gps_lnav_restore_almanac_page(alm, 5u),
"restoring an almanac page failed");
uint32_t mask = gps_lnav_get_almanac_pages(pages);
CHECK((mask & (1u << 4)) != 0u,
"restored almanac page not present in the raw-word cache");
for(unsigned int i = 0u; i < 10u; i++)
{
CHECK(pages[4][i] == (uint32_t)alm[i],
"restored almanac raw words differ from what was restored");
}
/*
* Toe carries no week on the air. The decoder reconstructs it from the
* subframe 1 week number and the handover word of the subframe that
* carried Toe, which is the only self-consistent pair available: Toe is
* within a couple of hours of its own transmission, so the week that puts
* it closest to that transmission is the right one.
*
* Without this the curve-fit age check works modulo one week and a record
* exactly N weeks old reads as current - harmless while ephemeris lives in
* RAM, reachable the moment it is persisted across a power-off.
*/
{
struct { const char* name; unsigned int wn; double how_tow;
double toe_sec; int expect_week; } cases[] = {
/* HOW TOW is a count of six-second units and Toe is a 16-bit
* field scaled by 16, so both are quantised on the air. */
{"mid-week", 430u, 172800.0, 172800.0, 430},
{"toe two hours ahead", 430u, 172800.0, 180000.0, 430},
/* Broadcast at the end of the week, Toe early in the next one. */
{"toe crosses forward", 430u, 604194.0, 1808.0, 431},
/* Broadcast just after the rollover, Toe late in the previous. */
{"toe crosses back", 431u, 1806.0, 604192.0, 430},
};
for(unsigned int c = 0u; c < 4u; c++)
{
gps_frame_t frame;
unsigned int s1[10], s2[10], s3[10];
memset(&frame, 0, sizeof(frame));
memset(s1, 0, sizeof(s1));
memset(s2, 0, sizeof(s2));
memset(s3, 0, sizeof(s3));
/* HOW: TOW count in six-second units, then the subframe id. */
unsigned int how = (unsigned int)(cases[c].how_tow / 6.0);
s1[1] = (how << 13) | (1u << 8);
s2[1] = (how << 13) | (2u << 8);
s3[1] = (how << 13) | (3u << 8);
/* Subframe 1: week number and the IODC whose low byte is the IODE. */
s1[2] = (cases[c].wn << 20);
s1[7] = (0x64u << 22);
/* Subframes 2 and 3 carry the matching IODE, and 2 carries Toe. */
s2[2] = (0x64u << 22);
s2[9] = (((unsigned int)(cases[c].toe_sec / 16.0)) << 14);
s3[9] = (0x64u << 22);
parse_subframe(s1, &frame, NULL);
parse_subframe(s2, &frame, NULL);
parse_subframe(s3, &frame, NULL);
CHECK(frame.gps_ephems.ephemeris_valid,
"consistent ephemeris set was not committed");
CHECK(frame.gps_ephems.toe_week_valid,
"toe week was not resolved");
if(frame.gps_ephems.toe.week != cases[c].expect_week)
{
fprintf(stderr, "FAIL: %s: toe week %d, expected %d\n",
cases[c].name, frame.gps_ephems.toe.week,
cases[c].expect_week);
failures++;
}
CHECK(frame.gps_ephems.toe.sec == cases[c].toe_sec,
"toe seconds did not survive the round trip");
}
}
if(failures==0) puts("gps lnav iode tests passed");
return failures==0?0:1;
}
soc/software/gps_system/tests/test_gps_measurement_model.c
+428
View File
@@ -0,0 +1,428 @@
/*
* The measurement model: how much one satellite is allowed to move the fix.
*
* Three quantities were being conflated in this code and it is worth naming
* them here, because only one of them is a bias:
*
* - the corrections, tropo() and klobuchar_delay_meters(), which are
* subtracted from the observation to remove a real physical delay;
* - the sigma, gps_nav_measurement_sigma_m(), which is not a bias at all
* and never shifts anything - it only says how much to trust a satellite;
* - the WLS clock bias, the fourth unknown, which is solved for.
*
* The defects these tests pin down were in the first two: a weighting floor
* that stopped the sigma model working below 10 degrees, and corrections that
* switched themselves off below the horizon and left their whole magnitude in
* the measurement as an uncorrected bias.
*/
#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include "gps/gps_ip_defines.h"
#include "gps/NavEquations.h"
#include "gps/matrixLib.h"
static int failures;
#define CHECK(condition, message) do { \
if(!(condition)) { \
fprintf(stderr, "FAIL: %s\n", message); \
failures++; \
} \
} while(0)
#define URA_INDEX 2u /* 4.85 m, a typical broadcast value */
/*
* The sigma model has to keep working where a satellite is worst. The floor
* used to be 10 degrees, which pinned the elevation term at 17.3 m: at 5
* degrees the model's own answer is 34.4 m and at 2 degrees it is 86 m, so a
* low satellite was trusted 4 to 25 times more than this same model said it
* should be. Nothing was ever excluded by that floor - it only stopped the
* mapping from doing its job.
*/
static void test_sigma_keeps_degrading_below_ten_degrees(void)
{
const double elevations[] = {90.0, 45.0, 30.0, 15.0, 10.0, 7.0, 5.0, 3.0};
double previous = 0.0;
printf(" elevation -> sigma relative weight vs zenith\n");
double zenith = gps_nav_measurement_sigma_m(URA_INDEX, 90.0, true,
NAN, false);
for(unsigned int i = 0u; i < (sizeof(elevations) / sizeof(elevations[0]));
i++)
{
double sigma = gps_nav_measurement_sigma_m(
URA_INDEX, elevations[i], true, NAN, false);
printf(" %5.1f deg -> %8.1f m %8.4f\n",
elevations[i], sigma, (zenith * zenith) / (sigma * sigma));
CHECK(isfinite(sigma) && (sigma > 0.0),
"sigma must stay finite and positive at every elevation");
if(i != 0u)
{
CHECK(sigma > previous,
"sigma stopped growing as elevation fell");
}
previous = sigma;
}
/* The model's own numbers, now that nothing truncates them. */
double at_ten = gps_nav_measurement_sigma_m(URA_INDEX, 10.0, true,
NAN, false);
double at_five = gps_nav_measurement_sigma_m(URA_INDEX, 5.0, true,
NAN, false);
CHECK(at_five > (1.8 * at_ten),
"a five degree satellite is still weighted like a ten degree one");
printf(" 5 deg sigma %.1f m vs 10 deg %.1f m: %.1fx the variance\n",
at_five, at_ten, (at_five * at_five) / (at_ten * at_ten));
}
/*
* The floor exists because 1/sin diverges at the horizon and a zero weight is
* not a discounted satellite - least_squares_build_postfit() refuses a
* non-positive weight and the whole epoch fails. So however low a satellite
* goes, the sigma must stay finite, positive, and monotone.
*/
static void test_sigma_is_bounded_at_and_below_the_horizon(void)
{
const double elevations[] = {1.0, 0.5, 0.0, -1.0, -5.0, -20.0};
for(unsigned int i = 0u; i < (sizeof(elevations) / sizeof(elevations[0]));
i++)
{
double sigma = gps_nav_measurement_sigma_m(
URA_INDEX, elevations[i], true, NAN, false);
CHECK(isfinite(sigma) && (sigma > 0.0),
"sigma went non-finite or non-positive below the horizon");
}
/* Below the horizon the correction is extrapolated and the signal has
* probably arrived by reflection, so it must cost more than merely being
* low. */
double just_above = gps_nav_measurement_sigma_m(URA_INDEX, 0.5, true,
NAN, false);
double just_below = gps_nav_measurement_sigma_m(URA_INDEX, -0.5, true,
NAN, false);
CHECK(just_below > just_above,
"a satellite below the horizon was not charged for it");
/* A non-finite elevation is unknown, not good: it must not be treated as
* a healthy satellite. */
double unknown = gps_nav_measurement_sigma_m(URA_INDEX, NAN, true,
NAN, false);
double at_thirty = gps_nav_measurement_sigma_m(URA_INDEX, 30.0, true,
NAN, false);
CHECK(unknown > at_thirty,
"a non-finite elevation was treated as a good satellite");
printf(" horizon bounded: 0.5 deg %.1f m, -0.5 deg %.1f m, "
"unknown %.1f m\n", just_above, just_below, unknown);
}
/*
* DLL code-tracking jitter goes as the inverse square root of carrier-to-noise
* density in linear power, so 20 dB less signal is 100 times less power and
* exactly ten times the sigma. This checks the law rather than the constant.
*/
static void test_cn0_follows_the_tracking_law(void)
{
/* Isolate the C/N0 term by holding elevation at zenith. */
double base = gps_nav_measurement_sigma_m(URA_INDEX, 90.0, true,
45.0, true);
double minus20 = gps_nav_measurement_sigma_m(URA_INDEX, 90.0, true,
25.0, true);
double ura = 4.85;
double zen = 3.0;
double common = (ura * ura) + (zen * zen);
double term_base = sqrt((base * base) - common);
double term_minus20 = sqrt((minus20 * minus20) - common);
printf(" C/N0 45 dB-Hz -> %.2f m, 25 dB-Hz -> %.2f m (ratio %.2f)\n",
term_base, term_minus20, term_minus20 / term_base);
CHECK(fabs(term_base - 3.0) < 1e-9,
"the C/N0 term is not 3 m at 45 dB-Hz");
CHECK(fabs((term_minus20 / term_base) - 10.0) < 1e-6,
"20 dB of C/N0 did not scale sigma by ten");
/* A weaker satellite is always trusted less, never more. */
double previous = 0.0;
for(double cn0 = 50.0; cn0 >= 28.0; cn0 -= 2.0)
{
double sigma = gps_nav_measurement_sigma_m(URA_INDEX, 90.0, true,
cn0, true);
CHECK(sigma > previous, "sigma did not grow as C/N0 fell");
previous = sigma;
}
}
/*
* The correction discontinuity.
*
* tropo() used to be called only for elevation above zero and Klobuchar
* returned zero at or below it, so a satellite crossing the horizon lost its
* entire atmospheric correction in one step - tens of metres appearing in the
* measurement as an uncorrected bias, from one epoch to the next, for no
* physical reason. Both are now evaluated at the horizon instead.
*
* tropo() is the public half of that pair, so the continuity is checked
* directly on it; the ionospheric half is exercised through the solve below.
*/
static void test_troposphere_is_continuous_through_the_horizon(void)
{
const double dtr = 0.01745329251994329576923690768489;
double above = tropo(sin(0.05 * dtr), 0.9);
double at = tropo(sin(0.0), 0.9);
double below = tropo(sin(-0.05 * dtr), 0.9);
printf(" troposphere at +0.05/0.00/-0.05 deg: %.2f / %.2f / %.2f m\n",
above, at, below);
CHECK(isfinite(above) && isfinite(at) && isfinite(below),
"the tropospheric model went non-finite near the horizon");
/*
* The model is genuinely steep here - about 30 m per degree - so this is
* about continuity, not flatness. What used to happen is the whole value
* dropping to zero the moment elevation went non-positive.
*/
CHECK(fabs(above - at) < 5.0,
"the tropospheric model steps across the horizon");
CHECK(fabs(below - at) < 1.0e-9,
"the tropospheric model is not clamped at the horizon");
CHECK(at > 20.0,
"the horizon tropospheric delay is implausibly small; dropping it "
"would leave that much uncorrected bias");
printf(" -> below the horizon it holds %.1f m instead of dropping to 0\n",
below);
}
/* ------------------------------------------------------------------ */
/* End to end: what a biased low satellite does to the fix. */
/* ------------------------------------------------------------------ */
/* Receiver near 40 N, 33 E, on the ellipsoid. */
static const double RECEIVER[3] = {4121000.0, 2699000.0, 4037000.0};
#define TRUE_CLOCK_BIAS_M 12345.0
/*
* Place a satellite at a given azimuth/elevation as seen from RECEIVER, at
* GPS orbital radius, by walking out along the local line of sight.
*/
static void place_satellite(double azimuth_deg, double elevation_deg,
double out[3])
{
const double dtr = 0.01745329251994329576923690768489;
double r = sqrt((RECEIVER[0] * RECEIVER[0]) +
(RECEIVER[1] * RECEIVER[1]) +
(RECEIVER[2] * RECEIVER[2]));
double ground = sqrt((RECEIVER[0] * RECEIVER[0]) +
(RECEIVER[1] * RECEIVER[1]));
double sin_lat = RECEIVER[2] / r;
double cos_lat = ground / r;
double sin_lon = RECEIVER[1] / ground;
double cos_lon = RECEIVER[0] / ground;
double el = elevation_deg * dtr;
double az = azimuth_deg * dtr;
double up = sin(el);
double north = cos(el) * cos(az);
double east = cos(el) * sin(az);
/* Local east/north/up back into ECEF. */
double dx = (-sin_lon * east) + (-sin_lat * cos_lon * north) +
(cos_lat * cos_lon * up);
double dy = (cos_lon * east) + (-sin_lat * sin_lon * north) +
(cos_lat * sin_lon * up);
double dz = (cos_lat * north) + (sin_lat * up);
/* Range to a satellite at 26560 km geocentric radius on that bearing. */
double b = 2.0 * ((RECEIVER[0] * dx) + (RECEIVER[1] * dy) +
(RECEIVER[2] * dz));
double c = (r * r) - (26560000.0 * 26560000.0);
double range = (-b + sqrt((b * b) - (4.0 * c))) / 2.0;
out[0] = RECEIVER[0] + (range * dx);
out[1] = RECEIVER[1] + (range * dy);
out[2] = RECEIVER[2] + (range * dz);
}
/*
* Solve one epoch and report how far the fix ends up from the truth.
* bias_row/bias_m inject an uncorrected error on one satellite, which is what
* a satellite whose atmospheric correction was dropped actually looks like.
*/
static bool solve_epoch(const double azimuth[], const double elevation[],
unsigned int count, int bias_row, double bias_m,
double solved_ecef_m[3], double* solved_clock_m)
{
mat_type* satpos = create_matrix((int)count, 3, "satpos");
mat_type* obs = create_matrix(1, (int)count, "obs");
mat_type* az = create_matrix(1, (int)count, "az");
mat_type* el = create_matrix(1, (int)count, "el");
unsigned char ura[GPS_TRCK_IP_COUNT];
gps_nav_solution_t solution;
bool ok = false;
if((satpos == NULL) || (obs == NULL) || (az == NULL) || (el == NULL))
{
goto done;
}
initialize_all(0.0, satpos);
initialize_all(0.0, obs);
initialize_all(0.0, az);
initialize_all(0.0, el);
for(unsigned int i = 0u; i < count; i++)
{
double sat[3];
place_satellite(azimuth[i], elevation[i], sat);
for(unsigned int k = 0u; k < 3u; k++)
{
satpos->matrix[i][k] = sat[k];
}
double dx = sat[0] - RECEIVER[0];
double dy = sat[1] - RECEIVER[1];
double dz = sat[2] - RECEIVER[2];
double range = sqrt((dx * dx) + (dy * dy) + (dz * dz));
obs->matrix[0][i] = range + TRUE_CLOCK_BIAS_M;
if((bias_row >= 0) && ((unsigned int)bias_row == i))
{
obs->matrix[0][i] += bias_m;
}
ura[i] = URA_INDEX;
}
/* No ionosphere model and no C/N0, so the only thing separating these
* satellites is geometry and the elevation weighting under test. */
if(!leastSquaresSTD(satpos, obs, (int)count, az, el, ura, NULL,
NULL, 300000.0, &solution))
{
goto done;
}
for(unsigned int k = 0u; k < 3u; k++)
{
solved_ecef_m[k] = solution.ecef_m[k];
}
*solved_clock_m = solution.clock_bias_m;
ok = true;
done:
free_matrix(satpos);
free_matrix(obs);
free_matrix(az);
free_matrix(el);
return ok;
}
/*
* Six satellites, one of them at 4 degrees. Clean, the solve must recover the
* truth; with 30 m of uncorrected delay on the low one, the fix must move by
* much less than that, because the weighting now knows how little that
* satellite is worth.
*
* The 30 m is not arbitrary: it is roughly the tropospheric delay that used to
* be dropped outright the moment a satellite crossed the horizon.
*/
static double displacement(const double a[3], const double b[3])
{
double dx = a[0] - b[0];
double dy = a[1] - b[1];
double dz = a[2] - b[2];
return sqrt((dx * dx) + (dy * dy) + (dz * dz));
}
/*
* Six satellites, one of them at 4 degrees. Inject 30 m of uncorrected delay
* on one of them and measure how far the fix moves - the displacement between
* two solves, not the distance from truth, because the synthetic observations
* deliberately carry no atmosphere and the model's own correction is a common
* offset that says nothing about the question being asked.
*
* The 30 m is not arbitrary: it is roughly the tropospheric delay that used to
* be dropped outright the moment a satellite crossed the horizon.
*
* The property under test is that the solve is *less* sensitive to a bias on a
* satellite it should not trust than to the same bias on one it should. With
* the elevation term floored at 10 degrees the two were nearly equal, which is
* what made a setting satellite as damaging as an overhead one.
*/
static void test_a_biased_low_satellite_moves_the_fix_less_than_its_bias(void)
{
const double azimuth[6] = { 10.0, 75.0, 140.0, 205.0, 280.0, 330.0};
const double elevation[6] = { 72.0, 41.0, 28.0, 55.0, 33.0, 4.0};
double clean[3];
double clean_clock = 0.0;
if(!solve_epoch(azimuth, elevation, 6u, -1, 0.0, clean, &clean_clock))
{
fprintf(stderr, "FAIL: the clean epoch did not solve\n");
failures++;
return;
}
double low[3];
double low_clock = 0.0;
double high[3];
double high_clock = 0.0;
if(!solve_epoch(azimuth, elevation, 6u, 5, 30.0, low, &low_clock) ||
!solve_epoch(azimuth, elevation, 6u, 0, 30.0, high, &high_clock))
{
fprintf(stderr, "FAIL: a biased epoch did not solve\n");
failures++;
return;
}
double low_move = displacement(low, clean);
double high_move = displacement(high, clean);
printf(" 30 m of uncorrected delay moves the fix by %.2f m on the "
"4 deg satellite, %.2f m on the 72 deg one\n",
low_move, high_move);
printf(" and the clock bias by %.2f m / %.2f m\n",
fabs(low_clock - clean_clock), fabs(high_clock - clean_clock));
/*
* What the ten degree floor did to that same satellite: it pinned the
* elevation term at 3/sin(10 deg) instead of 3/sin(4 deg), so the
* satellite entered the normal equations with several times the weight
* this model says it has earned - and the fix moved proportionally more.
*/
{
const double dtr = 0.01745329251994329576923690768489;
double ura = 4.85;
double floored = sqrt((ura * ura) +
pow(3.0 / sin(10.0 * dtr), 2.0));
double honest = gps_nav_measurement_sigma_m(URA_INDEX, 4.0, true,
NAN, false);
printf(" the 10 deg floor gave that satellite sigma %.1f m instead "
"of %.1f m: %.1fx the weight\n",
floored, honest, (honest * honest) / (floored * floored));
CHECK(honest > floored,
"the floor being regressed against no longer reproduces");
}
CHECK(low_move > 0.0, "the biased solve did not move at all");
CHECK(low_move < high_move,
"a bias on the lowest satellite pulled the fix at least as hard as "
"the same bias on the highest one");
CHECK(low_move < 30.0,
"a 30 m bias on the lowest satellite moved the fix by more than the "
"bias itself");
}
int main(void)
{
test_sigma_keeps_degrading_below_ten_degrees();
test_sigma_is_bounded_at_and_below_the_horizon();
test_cn0_follows_the_tracking_law();
test_troposphere_is_continuous_through_the_horizon();
test_a_biased_low_satellite_moves_the_fix_less_than_its_bias();
if(failures != 0)
{
fprintf(stderr, "%d check(s) failed\n", failures);
return 1;
}
printf("gps measurement model tests passed\n");
return 0;
}
soc/software/gps_system/tests/test_gps_nav_context_audit.c
+754
View File
@@ -0,0 +1,754 @@
/*
* Audit regressions for the persistent navigation context.
*
* Each test here reproduces a defect that was found by reading the code and
* confirmed by running it, and each one fails against the code as it stood
* before the accompanying fix. They are grouped in one binary because they
* share one story: persistence changed the lifetime of the almanac, the
* ionosphere page and the last position from "one power cycle" to "weeks", and
* three separate pieces of arithmetic were only ever correct over the shorter
* lifetime.
*
* The flash side runs against the same simulated SPI NOR as
* test_gps_nav_context_store.c: programming can only clear bits and a program
* into a non-erased page corrupts it.
*/
#define _POSIX_C_SOURCE 200809L
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/wait.h>
#include "gps/gps_ip_defines.h"
#include "gps/gps_almanac.h"
#include "gps/gps_ephemeris_cache.h"
#include "gps/gps_ephemeris_validity.h"
#include "gps/gps_nav_context_store.h"
static int failures;
#define CHECK(condition, message) do { \
if(!(condition)) { \
fprintf(stderr, "FAIL: %s\n", message); \
failures++; \
} \
} while(0)
/* ------------------------------------------------------------------ */
/* LNAV word construction, shared with test_gps_almanac.c conventions. */
/* ------------------------------------------------------------------ */
static void put_field(unsigned int* word, unsigned int first_bit,
unsigned int length, uint32_t value)
{
unsigned int shift = 30u - (first_bit + length - 1u);
uint32_t mask = (length >= 32u) ? 0xffffffffu : ((1u << length) - 1u);
*word &= ~(mask << shift);
*word |= (value & mask) << shift;
}
/* Subframe 5 page 25: the almanac reference week (WNa) and toa. */
static void build_page25(unsigned int subframe[10], unsigned int wna,
unsigned int toa_raw)
{
memset(subframe, 0, sizeof(unsigned int) * 10u);
put_field(&subframe[1], 20u, 3u, 5u); /* HOW subframe id */
put_field(&subframe[2], 1u, 2u, 1u); /* data ID 1 = LNAV */
put_field(&subframe[2], 3u, 6u, 51u); /* SV ID 51 = page 25 */
put_field(&subframe[2], 9u, 8u, toa_raw);
put_field(&subframe[2], 17u, 8u, wna);
}
/*
* The almanac reference week is broadcast as WNa, the eight least significant
* bits of the week, and has to be lifted against a week the receiver already
* knows. The lift ran against gps_ephems.toe.week, and nothing in the
* receiver ever writes that field - LNAV broadcasts Toe as a second-of-week
* only. Lifting against the zero it holds put the result 256 weeks below the
* receiver for every WNa above 128, which in unsigned arithmetic underflowed
* to about 4.29e9.
*
* GPS week 2431 (the week this was found in) has WNa 127, so the fault was
* two weeks in the future rather than present: it arms at week 2433 and then
* for 127 weeks out of every 256.
*/
static void test_almanac_reference_week_is_lifted_sanely(void)
{
const int frame_week = 383; /* the 10-bit broadcast scale: 2431 & 1023 */
/*
* The arithmetic as it stood, reproduced here so the defect is stated and
* not just described. Run against the real parser before the fix this
* produced 4294967169 for WNa 129, and gps_almanac_satellite_ecef() then
* spent 4.2 s of a 5 GHz x86 walking it back one week at a time.
*/
{
unsigned int broken_frame_week = 0u; /* toe.week: never written */
unsigned int broken =
(broken_frame_week & ~0xffu) | (129u & 0xffu);
if((broken + 128u) < broken_frame_week) { broken += 256u; }
else if(broken > (broken_frame_week + 128u)) { broken -= 256u; }
printf(" the previous lift gave week %u for WNa 129 "
"(receiver week %d)\n", broken, frame_week);
CHECK(broken > 1000000u,
"the defect being regressed against no longer reproduces");
}
for(unsigned int wna = 0u; wna <= 255u; wna++)
{
gps_frame_t frame;
unsigned int subframe[10];
memset(&frame, 0, sizeof(frame));
frame.gps_ephems.tow.week = frame_week;
build_page25(subframe, wna, 36u);
gps_lnav_reset_almanac();
parse_subframe(subframe, &frame, NULL);
const gps_almanac_t* almanac = gps_lnav_get_almanac();
if(!almanac->reference_valid)
{
continue; /* refusing to adopt a week is always allowed */
}
long delta = (long)almanac->week - (long)frame_week;
if((delta > 128) || (delta < -128))
{
fprintf(stderr,
"FAIL: wna=%u lifted to week %u, %ld weeks from the "
"receiver's %d\n",
wna, almanac->week, delta, frame_week);
failures++;
return;
}
}
printf(" WNa lifts to within +/-128 weeks for all 256 values\n");
}
/*
* The consequence of the above, and the reason it is a hang and not a wrong
* answer. Propagation reduced tk with `while(tk < -302400) tk += 604800;`, so
* a week number 4.29e9 out cost 4.29e9 iterations - measured at 4.2 s per
* satellite on a 5 GHz x86, which on a 50 MHz in-order Rocket core is hours,
* times 32 satellites, inside the 10 s visibility refresh.
*
* Run it in a child under a wall-clock limit so a regression shows up as a
* failed test rather than as a hung test run.
*/
static void test_propagation_cannot_take_unbounded_time(void)
{
gps_almanac_entry_t entry;
memset(&entry, 0, sizeof(entry));
entry.valid = true;
entry.prn = 1u;
entry.sqrt_a = 5153.6;
entry.ecc = 0.006;
entry.toa_sec = 147456.0;
entry.i0_rad = (0.30 + 0.0047) * 3.1415926535897932;
entry.omega_dot_rps = -8.0e-9;
entry.omega0_rad = 1.0;
entry.omega_rad = 0.5;
entry.m0_rad = 2.0;
pid_t pid = fork();
if(pid == 0)
{
double ecef[3];
alarm(5);
/* Every week number a corrupt lift can produce. */
(void)gps_almanac_satellite_ecef(&entry, 4294967169u, 383u,
300000.0, ecef);
(void)gps_almanac_satellite_ecef(&entry, 0u, 4294967169u,
300000.0, ecef);
(void)gps_almanac_satellite_ecef(&entry, 383u, 0u, 300000.0, ecef);
_exit(0);
}
CHECK(pid > 0, "fork");
int status = 0;
(void)waitpid(pid, &status, 0);
CHECK(WIFEXITED(status) && (WEXITSTATUS(status) == 0),
"propagation with a corrupt week number did not return promptly");
printf(" propagation is closed form for every corrupt week number\n");
}
/*
* A week number that cannot be a GPS week must not be adopted at all, from the
* air or from flash. Without this a single bad record on the flash would keep
* the visibility model switched off across every future boot.
*/
static void test_impossible_reference_week_is_refused(void)
{
gps_almanac_t almanac;
gps_almanac_reset(&almanac);
CHECK(!gps_almanac_set_reference(&almanac, 4294967169u, 147456.0),
"an impossible week was adopted");
CHECK(!almanac.reference_valid, "an impossible week was marked valid");
CHECK(!gps_almanac_set_reference(&almanac, 2431u, 604800.0),
"a toa outside the week was adopted");
CHECK(gps_almanac_set_reference(&almanac, 2431u, 147456.0),
"a real reference was refused");
CHECK(almanac.reference_valid, "a real reference was not marked valid");
printf(" impossible almanac references refused\n");
}
/*
* The half-week wrap was applied to tk unconditionally, described as
* IS-GPS-200 20.3.3.5.2.3. That paragraph is a week-crossover correction, not
* a general reduction, and applying it as one is wrong because a GPS week is
* 14.0384 orbital periods rather than 14: folding a week away moves the
* satellite about 14 degrees along its orbit.
*
* Verified against an independent Python implementation of Table 20-IV: for
* the almanac in test_gps_almanac.c, aliasing one week displaces the satellite
* by 5161 km, and from a receiver at 40 N a satellite genuinely 5.1 degrees
* below the horizon is reported 5.7 degrees above it. Persistence is what
* made this reachable: an almanac read back from flash is routinely older than
* half a week, whereas one collected live never is.
*/
static void test_stale_almanac_is_refused_not_aliased(void)
{
gps_almanac_entry_t entry;
memset(&entry, 0, sizeof(entry));
entry.valid = true;
entry.prn = 1u;
entry.ecc = 3902.0 * ldexp(1.0, -21);
entry.toa_sec = 36.0 * 4096.0;
entry.i0_rad = (0.30 + (2439.0 * ldexp(1.0, -19))) * 3.1415926535897932;
entry.omega_dot_rps = -691.0 * ldexp(1.0, -38) * 3.1415926535897932;
entry.sqrt_a = 10554475.0 * ldexp(1.0, -11);
entry.omega0_rad = 677285.0 * ldexp(1.0, -23) * 3.1415926535897932;
entry.omega_rad = 459593.0 * ldexp(1.0, -23) * 3.1415926535897932;
entry.m0_rad = 301402.0 * ldexp(1.0, -23) * 3.1415926535897932;
/* Receiver near 40 N, 33 E. */
const double receiver[3] = {4121000.0, 2699000.0, 4037000.0};
/*
* 5.784 days after toa, expressed honestly as week 2432. The old code
* folded tk back to -1.216 days and answered +5.67 degrees; the truth is
* -5.10 degrees, on the other side of the -5 degree acquisition mask.
*/
double elevation = 0.0;
CHECK(gps_almanac_look_angles(&entry, 2431u, 2432u, 42456.0, receiver,
&elevation, NULL),
"a 5.8 day old almanac should still propagate");
CHECK(fabs(elevation - (-5.10)) < 0.2,
"a 5.8 day old almanac was aliased by a week instead of propagated");
/*
* What the unconditional wrap evaluated instead. tk was 499800 s; the
* wrap folded it to -105000 s, which is the same as asking for the
* satellite one week earlier - so it can be reproduced exactly by naming
* that earlier instant.
*/
double aliased = 0.0;
CHECK(gps_almanac_look_angles(&entry, 2431u, 2431u, 42456.0, receiver,
&aliased, NULL),
"alias reference case");
printf(" 5.8 day old almanac: elevation %+.2f deg, "
"one-week alias says %+.2f deg (%.1f deg apart)\n",
elevation, aliased, fabs(elevation - aliased));
CHECK(fabs(elevation - aliased) > 10.0,
"the aliasing being regressed against no longer reproduces");
CHECK((elevation < GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG) &&
(aliased > GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG),
"this case no longer straddles the acquisition elevation mask");
/* Beyond the usable window there must be no answer at all, so the PRN
* stays fully searchable rather than being demoted on a guess. */
double ignored = 0.0;
CHECK(!gps_almanac_look_angles(&entry, 2431u, 2435u, 42456.0, receiver,
&ignored, NULL),
"an almanac 26 days old produced a look angle");
printf(" 26 day old almanac produces no prediction\n");
}
/*
* Before subframe 5 page 25 arrives there is no WNa, and the receiver has to
* assume the almanac is current. That assumption belongs in one place and has
* to reproduce the answer the real week gives, including across a week
* boundary - otherwise the visible set would step every Sunday morning.
*/
static void test_assumed_week_matches_the_real_one(void)
{
gps_almanac_entry_t entry;
memset(&entry, 0, sizeof(entry));
entry.valid = true;
entry.toa_sec = 589824.0; /* late in the week */
/* Same instant, one week later in the numbering: toa is in the previous
* week, so the guess has to reach back for it. */
CHECK(gps_almanac_assumed_week(&entry, 2432u, 100.0) == 2431u,
"assumed week did not reach back across the week boundary");
CHECK(gps_almanac_assumed_week(&entry, 2431u, 600000.0) == 2431u,
"assumed week moved when it should not have");
entry.toa_sec = 4096.0; /* early in the week */
CHECK(gps_almanac_assumed_week(&entry, 2431u, 600000.0) == 2432u,
"assumed week did not reach forward across the week boundary");
printf(" assumed almanac week is exact either side of a rollover\n");
}
/*
* Health only ever demotes, so it must not be trusted further than the orbit
* it came with. Since persistence an entry can be weeks old and still carry a
* health byte that was true when it was written and is not now; an almanac too
* old to say where a satellite is cannot be allowed to say it is sick.
*/
static void test_stale_health_does_not_demote(void)
{
gps_almanac_t almanac;
uint32_t mask = 0u;
const double receiver[3] = {4121000.0, 2699000.0, 4037000.0};
gps_almanac_reset(&almanac);
almanac.sv[0].valid = true;
almanac.sv[0].prn = 1u;
almanac.sv[0].health = 0x3fu; /* marked unhealthy long ago */
almanac.sv[0].ecc = 0.006;
almanac.sv[0].toa_sec = 147456.0;
almanac.sv[0].i0_rad = 0.3047 * 3.1415926535897932;
almanac.sv[0].sqrt_a = 5153.6;
almanac.sv[0].omega0_rad = 1.0;
almanac.sv[0].omega_rad = 0.5;
almanac.sv[0].m0_rad = 2.0;
/* Give it a second entry so the mask is not the all-ones fallback. */
almanac.sv[1] = almanac.sv[0];
almanac.sv[1].prn = 2u;
almanac.sv[1].health = 0u;
gps_almanac_set_reference(&almanac, 2431u, 147456.0);
/* Fresh almanac: an unhealthy satellite is worth de-prioritising. */
(void)gps_almanac_visible_mask(&almanac, 2431u, 200000.0, receiver,
-5.0, &mask);
CHECK((mask & 1u) == 0u,
"a fresh unhealthy satellite was not demoted");
/* Four weeks later the same health byte proves nothing. */
(void)gps_almanac_visible_mask(&almanac, 2435u, 200000.0, receiver,
-5.0, &mask);
CHECK((mask & 1u) != 0u,
"a four-week-old health byte still demoted a satellite");
printf(" stale health no longer demotes; fresh health still does\n");
}
/*
* The seed persisted to flash says where the receiver was, and a receiver that
* was carried somewhere else while switched off has no way to know that until
* it has a fix. Aiming the acquisition mask at the old site would then demote
* satellites that are up at the new one, which is the one failure mode the
* whole feature is supposed to avoid.
*
* Measure it: build a realistic constellation, take the true visible set at
* the new site, and check what a mask computed from the old seed would say.
* The widened seed mask has to keep every genuinely visible satellite
* searchable at the fast cadence.
*/
static void test_migration_between_power_cycles(void)
{
gps_almanac_t almanac;
gps_almanac_reset(&almanac);
for(unsigned int i = 0u; i < GPS_ALMANAC_PRN_COUNT; i++)
{
gps_almanac_entry_t* e = &almanac.sv[i];
e->valid = true;
e->prn = (unsigned char)(i + 1u);
e->health = 0u;
e->ecc = 0.006;
e->toa_sec = 147456.0;
e->i0_rad = 0.3047 * 3.1415926535897932;
e->sqrt_a = 5153.6;
/* Six planes, spread within each - the real constellation's shape. */
e->omega0_rad = (double)(i % 6u) * (2.0 * 3.1415926535897932 / 6.0);
e->omega_rad = 0.0;
e->m0_rad = (double)(i / 6u) * (2.0 * 3.1415926535897932 / 6.0) +
(double)(i % 6u) * 0.7;
}
gps_almanac_set_reference(&almanac, 2431u, 147456.0);
/* Powered off in Ankara, powered on 1600 km away in Berlin. */
const double seed[3] = {4121000.0, 2699000.0, 4037000.0};
const double actual[3] = {3783000.0, 901000.0, 5038000.0};
uint32_t truth = 0u;
uint32_t narrow = 0u;
uint32_t wide = 0u;
unsigned int visible = gps_almanac_visible_mask(
&almanac, 2431u, 300000.0, actual,
GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG, &truth);
(void)gps_almanac_visible_mask(&almanac, 2431u, 300000.0, seed,
GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG,
&narrow);
(void)gps_almanac_visible_mask(&almanac, 2431u, 300000.0, seed,
GPS_ACQ_VISIBILITY_SEED_MASK_ELEVATION_DEG,
&wide);
unsigned int missed_narrow = 0u;
unsigned int missed_wide = 0u;
for(unsigned int i = 0u; i < GPS_ALMANAC_PRN_COUNT; i++)
{
uint32_t bit = 1u << i;
if((truth & bit) == 0u)
{
continue;
}
if((narrow & bit) == 0u) { missed_narrow++; }
if((wide & bit) == 0u) { missed_wide++; }
}
printf(" 1600 km migration: %u satellites up at the new site; "
"old seed at %.0f deg demotes %u of them, at %.0f deg demotes %u\n",
visible, (double)GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG,
missed_narrow, (double)GPS_ACQ_VISIBILITY_SEED_MASK_ELEVATION_DEG,
missed_wide);
CHECK(missed_wide == 0u,
"the widened seed mask still demoted a visible satellite");
CHECK(missed_wide <= missed_narrow,
"widening the mask made the prediction worse");
}
/* ------------------------------------------------------------------ */
/* Flash: the wear policy against a realistic duty cycle. */
/* ------------------------------------------------------------------ */
#define SECTOR_SIZE 8192u
#define FLASH_SIZE (SECTOR_SIZE * 2u)
typedef struct {
uint8_t data[FLASH_SIZE];
uint32_t erase_count;
uint32_t program_count;
} sim_flash_t;
static void sim_read(void* ctx, uint32_t address, void* dst, uint32_t len)
{
memcpy(dst, &((sim_flash_t*)ctx)->data[address], len);
}
static bool sim_erase(void* ctx, uint32_t address)
{
sim_flash_t* f = (sim_flash_t*)ctx;
memset(&f->data[address], 0xff, SECTOR_SIZE);
f->erase_count++;
return true;
}
static bool sim_program(void* ctx, uint32_t address, const void* src,
uint32_t len)
{
sim_flash_t* f = (sim_flash_t*)ctx;
const uint8_t* in = (const uint8_t*)src;
for(uint32_t i = 0u; i < len; i++)
{
f->data[address + i] &= in[i]; /* programming only clears bits */
}
f->program_count++;
return true;
}
static void sim_bind(gps_ephemeris_store_flash_t* flash, sim_flash_t* sim)
{
memset(sim, 0, sizeof(*sim));
memset(sim->data, 0xff, sizeof(sim->data));
memset(flash, 0, sizeof(*flash));
flash->read = sim_read;
flash->erase_sector = sim_erase;
flash->program = sim_program;
flash->ctx = sim;
flash->base_address = 0u;
flash->sector_size = SECTOR_SIZE;
}
static void fill_static_almanac(gps_nav_context_payload_t* p)
{
for(unsigned int prn = 0u; prn < 24u; prn++)
{
for(unsigned int w = 0u; w < GPS_NAV_CONTEXT_WORDS; w++)
{
p->almanac_words[prn][w] = 0x11000000u + (prn * 97u) + w;
}
p->almanac_present_mask |= (1u << prn);
}
p->almanac_reference_valid = 1u;
p->almanac_week = 2431u;
p->almanac_toa_sec = 147456u;
p->iono_valid = 1u;
for(unsigned int w = 0u; w < GPS_NAV_CONTEXT_WORDS; w++)
{
p->iono_words[w] = 0x22000000u + w;
}
}
/*
* One day of a stationary receiver: the constellation broadcasts the same
* almanac, page 18 does not change, and the WLS position wanders by a few
* metres of noise around one point, exactly as the board's own log shows.
*
* The whole point of the store's CRC comparison is that this costs nothing.
* It did not: the payload carried the raw WLS position and the receiver time
* it was taken at, both of which move every epoch, so the CRC differed on
* every save and the erase count followed the 60 s timer. Board evidence:
* generation advanced 12 -> 17 across one short capture with a single skipped
* write. That is 1440 erase cycles a day against a 100k cycle endurance
* shared by two sectors: about 139 days to wear-out, to persist a seed that is
* only ever read once, at the next boot.
*/
#define SAVES_PER_DAY (24u * 60u) /* GPS_NAV_CONTEXT_SAVE_PERIOD_TICKS = 60 s */
/* The receiver stands still; the WLS fix scatters by metres, as it does on the
* board, and receiver time advances monotonically. */
static void stationary_fix(unsigned int i, double fix[3])
{
static const double truth[3] = {4121000.0, 2699000.0, 4037000.0};
fix[0] = truth[0] + (double)((int)(i % 17u) - 8);
fix[1] = truth[1] + (double)((int)(i % 11u) - 5);
fix[2] = truth[2] + (double)((int)(i % 23u) - 11);
}
/* One day of periodic saves, returning the number of sector erases. */
static uint32_t run_one_day(bool hold_position, uint32_t* skipped_out)
{
sim_flash_t sim;
gps_ephemeris_store_flash_t flash;
gps_nav_context_store_t store;
sim_bind(&flash, &sim);
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
double held[3] = {0.0, 0.0, 0.0};
bool held_valid = false;
uint32_t held_week = 0u;
double held_tow = 0.0;
uint64_t held_tick = 0u;
for(unsigned int i = 0u; i < SAVES_PER_DAY; i++)
{
uint64_t now = (uint64_t)(i + 1u) * 60u * GPS_SYSTICK_HZ;
double fix[3];
stationary_fix(i, fix);
bool take;
if(hold_position)
{
/* The policy gps_nav_context_flash_service() now applies. */
bool moved = !held_valid ||
gps_nav_context_position_changed(
held, fix, GPS_NAV_CONTEXT_POSITION_EPSILON_M);
bool due = (held_tick == 0u) ||
((now - held_tick) >= GPS_NAV_CONTEXT_POSITION_REFRESH_TICKS);
take = moved && due;
}
else
{
/* What it did before: take whatever the last fix said, every
* time, along with the receiver time it was taken at. */
take = true;
}
if(take)
{
held[0] = fix[0];
held[1] = fix[1];
held[2] = fix[2];
held_valid = true;
held_tick = now;
held_week = 2431u;
held_tow = 550000.0 + (double)i * 60.0;
}
gps_nav_context_payload_t payload;
memset(&payload, 0, sizeof(payload));
fill_static_almanac(&payload);
if(held_valid)
{
payload.position_valid = 1u;
payload.position_ecef_m[0] = held[0];
payload.position_ecef_m[1] = held[1];
payload.position_ecef_m[2] = held[2];
payload.position_week = held_week;
payload.position_tow_sec = held_tow;
}
bool wrote = false;
CHECK(gps_nav_context_store_save(&store, &payload, &wrote), "save");
}
if(skipped_out != NULL)
{
*skipped_out = store.skipped_writes;
}
return sim.erase_count;
}
static void test_stationary_receiver_does_not_wear_the_flash(void)
{
uint32_t before_skipped = 0u;
uint32_t after_skipped = 0u;
uint32_t before = run_one_day(false, &before_skipped);
uint32_t after = run_one_day(true, &after_skipped);
/* Two sectors alternate, so each sees half the cycles; S25FL256S is
* specified for 100k program/erase cycles per sector. */
printf(" 24 h stationary, %u saves: taking every fix -> %u erases "
"(%u skipped), %.1f years to 100k cycles\n",
SAVES_PER_DAY, before, before_skipped,
(before > 0u) ? (100000.0 * 2.0) / ((double)before * 365.0) : 0.0);
printf(" 24 h stationary, %u saves: held seed -> %u erases "
"(%u skipped), %.0f years to 100k cycles\n",
SAVES_PER_DAY, after, after_skipped,
(after > 0u) ? (100000.0 * 2.0) / ((double)after * 365.0) : 0.0);
CHECK(before >= (SAVES_PER_DAY - 1u),
"the write amplification being regressed against no longer reproduces");
CHECK(after <= 2u,
"a stationary receiver erased the flash more than twice in a day");
CHECK(after_skipped >= (SAVES_PER_DAY - 2u),
"unchanged saves were not skipped");
}
/*
* The other half of the same policy: a receiver that really has moved must
* still update its seed, otherwise a relocated receiver would cold start with
* a visibility prediction aimed at the old site for ever.
*/
static void test_a_moved_receiver_still_updates_its_seed(void)
{
const double from[3] = {4121000.0, 2699000.0, 4037000.0};
double to[3] = {4121000.0 + 60000.0, 2699000.0, 4037000.0};
CHECK(gps_nav_context_position_changed(from, to,
GPS_NAV_CONTEXT_POSITION_EPSILON_M),
"a 60 km move was not seen as a move");
to[0] = from[0] + 3.0;
CHECK(!gps_nav_context_position_changed(
from, to, GPS_NAV_CONTEXT_POSITION_EPSILON_M),
"3 m of WLS scatter was treated as a move");
double nan_position[3] = {0.0, 0.0, 0.0};
nan_position[0] = NAN;
CHECK(!gps_nav_context_position_changed(
from, nan_position, GPS_NAV_CONTEXT_POSITION_EPSILON_M),
"a non-finite position was accepted as a move");
printf(" seed refresh triggers on kilometres, not on scatter\n");
}
/* ------------------------------------------------------------------ */
/* Ephemeris restored from flash versus what is actually on the air. */
/* ------------------------------------------------------------------ */
static void make_ephemeris(gps_ephemeris_t* eph, unsigned int prn,
unsigned char iode, double toe_sec)
{
memset(eph, 0, sizeof(*eph));
eph->ephemeris_valid = true;
eph->SVID = (unsigned char)prn;
eph->iode = iode;
eph->iodc = iode;
eph->toe.sec = toe_sec;
eph->toc.sec = toe_sec;
eph->toe.week = 2431;
eph->toe_week_valid = true;
eph->A = 26560000.0;
eph->sqrta = 5153.6;
eph->ecc = 0.006;
eph->sva = 2u;
eph->svhlth = 0u;
eph->update = 0u;
}
/*
* Toe is broadcast as a second-of-week, so two Toe values are ordered with the
* signed half-week rule. That is exact between records from the same era and
* silently inverted once the gap exceeds half a week - which is precisely what
* persistence introduced. Switch the receiver off on Friday and on again on
* Tuesday and the four-day-old flash record outranks the live broadcast: the
* cache refuses the fresh data set, the flash keeps being rewritten with the
* stale one, and every subsequent cold start restores it again.
*/
static void test_broadcast_beats_a_record_read_back_from_flash(void)
{
gps_ephemeris_cache_t cache;
gps_ephemeris_t restored;
gps_ephemeris_t broadcast;
gps_ephemeris_t out;
/* A four-day gap: the raw Toe difference is 345600 s, past half a week,
* so the half-week rule reads the newer record as the older one. */
make_ephemeris(&restored, 5u, 0x40u, 100000.0);
make_ephemeris(&broadcast, 5u, 0x41u, 100000.0 + 345600.0 - 604800.0 +
604800.0);
broadcast.toe.sec = fmod(100000.0 + 345600.0, 604800.0);
CHECK(!gps_ephemeris_supersedes(&broadcast, &restored),
"the half-week rule was expected to mis-order these; test is stale");
gps_ephemeris_cache_init(&cache, 1000000u);
CHECK(gps_ephemeris_cache_store_restored(&cache, 5u, &restored, 10u),
"flash record not adopted");
bool changed = false;
CHECK(gps_ephemeris_cache_store_if_new(&cache, 5u, &broadcast, 20u,
&changed),
"live broadcast was refused in favour of a flash record");
CHECK(changed, "adopting the live broadcast was not reported as a change");
CHECK(gps_ephemeris_cache_restore(&cache, 5u, 30u, &out), "restore");
CHECK(out.iode == 0x41u, "the cache kept the stale flash record");
printf(" live broadcast replaces a stale flash record\n");
}
/*
* The flag must not become a way for flash to overwrite live data, and it must
* clear once used, so ordinary Toe ordering resumes for everything after.
*/
static void test_restored_never_displaces_live_data(void)
{
gps_ephemeris_cache_t cache;
gps_ephemeris_t live;
gps_ephemeris_t stale;
gps_ephemeris_t older;
gps_ephemeris_t out;
gps_ephemeris_cache_init(&cache, 1000000u);
make_ephemeris(&live, 7u, 0x20u, 300000.0);
make_ephemeris(&stale, 7u, 0x10u, 100000.0);
bool changed = false;
CHECK(gps_ephemeris_cache_store_if_new(&cache, 7u, &live, 10u, &changed),
"live record not stored");
CHECK(!gps_ephemeris_cache_store_restored(&cache, 7u, &stale, 20u),
"a flash record displaced live data");
CHECK(gps_ephemeris_cache_restore(&cache, 7u, 30u, &out), "restore");
CHECK(out.iode == 0x20u, "live record was lost");
/* And after one broadcast has cleared the flag, an older data set is
* rejected on Toe again as it always was. */
make_ephemeris(&older, 7u, 0x1fu, 290000.0);
CHECK(!gps_ephemeris_cache_store_if_new(&cache, 7u, &older, 40u, &changed),
"an older data set was adopted after the restored flag cleared");
printf(" restored records never outrank live ones\n");
}
int main(void)
{
test_almanac_reference_week_is_lifted_sanely();
test_propagation_cannot_take_unbounded_time();
test_impossible_reference_week_is_refused();
test_stale_almanac_is_refused_not_aliased();
test_assumed_week_matches_the_real_one();
test_stale_health_does_not_demote();
test_migration_between_power_cycles();
test_stationary_receiver_does_not_wear_the_flash();
test_a_moved_receiver_still_updates_its_seed();
test_broadcast_beats_a_record_read_back_from_flash();
test_restored_never_displaces_live_data();
if(failures != 0)
{
fprintf(stderr, "%d check(s) failed\n", failures);
return 1;
}
printf("gps nav context audit tests passed\n");
return 0;
}
soc/software/gps_system/tests/test_gps_nav_context_store.c
+350
View File
@@ -0,0 +1,350 @@
/*
* Persistent navigation context store against a simulated SPI NOR device.
*
* The simulator enforces what makes flash different from RAM: programming can
* only clear bits, a program into a non-erased page corrupts it, and a write
* can be cut short at any byte to model power loss.
*/
#include <assert.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "gps/gps_nav_context_store.h"
#define SECTOR_SIZE 8192u
#define SECTORS 2u
#define FLASH_SIZE (SECTOR_SIZE * SECTORS)
static int failures;
#define CHECK(condition, message) do { \
if(!(condition)) { \
fprintf(stderr, "FAIL: %s\n", message); \
failures++; \
} \
} while(0)
typedef struct {
uint8_t data[FLASH_SIZE];
uint32_t erase_count;
uint32_t program_count;
int32_t fail_after_programs;
uint32_t truncate_bytes;
} sim_flash_t;
static void sim_init(sim_flash_t* f)
{
memset(f, 0, sizeof(*f));
memset(f->data, 0xff, sizeof(f->data));
f->fail_after_programs = -1;
}
static void sim_read(void* ctx, uint32_t address, void* dst, uint32_t len)
{
sim_flash_t* f = (sim_flash_t*)ctx;
assert((address + len) <= FLASH_SIZE);
memcpy(dst, &f->data[address], len);
}
static bool sim_erase(void* ctx, uint32_t address)
{
sim_flash_t* f = (sim_flash_t*)ctx;
assert((address % SECTOR_SIZE) == 0u);
assert(address < FLASH_SIZE);
memset(&f->data[address], 0xff, SECTOR_SIZE);
f->erase_count++;
return true;
}
static bool sim_program(void* ctx, uint32_t address, const void* src,
uint32_t len)
{
sim_flash_t* f = (sim_flash_t*)ctx;
const uint8_t* in = (const uint8_t*)src;
assert((address + len) <= FLASH_SIZE);
/* A real page program cannot span a 256-byte page. */
assert(((address % 256u) + len) <= 256u);
uint32_t writable = len;
bool truncated = false;
if((f->fail_after_programs >= 0) &&
(f->program_count == (uint32_t)f->fail_after_programs))
{
writable = f->truncate_bytes;
truncated = true;
}
for(uint32_t i = 0u; i < writable; i++)
{
/* Programming can only clear bits. */
f->data[address + i] &= in[i];
}
f->program_count++;
return !truncated;
}
static void bind(gps_ephemeris_store_flash_t* flash, sim_flash_t* sim)
{
memset(flash, 0, sizeof(*flash));
flash->read = sim_read;
flash->erase_sector = sim_erase;
flash->program = sim_program;
flash->ctx = sim;
flash->base_address = 0u;
flash->sector_size = SECTOR_SIZE;
}
static void fill_payload(gps_nav_context_payload_t* p, uint32_t seed)
{
memset(p, 0, sizeof(*p));
for(unsigned int prn = 0u; prn < 8u; prn++)
{
for(unsigned int w = 0u; w < GPS_NAV_CONTEXT_WORDS; w++)
{
p->almanac_words[prn][w] = (seed * 7u) + (prn * 31u) + w;
}
p->almanac_present_mask |= (1u << prn);
}
p->almanac_reference_valid = 1u;
p->almanac_week = 2431u;
p->almanac_toa_sec = 147456u;
p->iono_valid = 1u;
for(unsigned int w = 0u; w < GPS_NAV_CONTEXT_WORDS; w++)
{
p->iono_words[w] = seed + w;
}
p->position_valid = 1u;
p->position_ecef_m[0] = 4128000.0 + seed;
p->position_ecef_m[1] = 2530000.0;
p->position_ecef_m[2] = 4136000.0;
p->position_week = 2431u;
p->position_tow_sec = 550000.0;
}
static void test_empty_mount_holds_nothing(void)
{
sim_flash_t sim;
gps_ephemeris_store_flash_t flash;
gps_nav_context_store_t store;
gps_nav_context_payload_t payload;
sim_init(&sim);
bind(&flash, &sim);
CHECK(gps_nav_context_store_mount(&store, &flash), "blank store mounts");
CHECK(!gps_nav_context_store_load(&store, &payload),
"blank store holds nothing");
CHECK(sim.erase_count == 0u, "mounting does not erase");
printf(" empty mount ok\n");
}
static void test_round_trip(void)
{
sim_flash_t sim;
gps_ephemeris_store_flash_t flash;
gps_nav_context_store_t store;
gps_nav_context_payload_t written;
gps_nav_context_payload_t read;
sim_init(&sim);
bind(&flash, &sim);
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
fill_payload(&written, 3u);
bool wrote = false;
CHECK(gps_nav_context_store_save(&store, &written, &wrote), "save");
CHECK(wrote, "first save writes");
CHECK(gps_nav_context_store_load(&store, &read), "load after save");
CHECK(memcmp(&written, &read, sizeof(written)) == 0,
"payload survives round trip");
/* And it survives a remount, which is the case that actually matters. */
gps_nav_context_store_t remounted;
CHECK(gps_nav_context_store_mount(&remounted, &flash), "remount");
memset(&read, 0, sizeof(read));
CHECK(gps_nav_context_store_load(&remounted, &read), "load after remount");
CHECK(memcmp(&written, &read, sizeof(written)) == 0,
"payload survives remount");
printf(" round trip ok\n");
}
/*
* The wear policy, and the property the feature was asked for: saving the same
* content again must not touch the flash at all. Almanac pages are rebroadcast
* continuously, so without this the erase count would follow the save timer
* rather than how often the constellation publishes new data.
*/
static void test_unchanged_save_does_not_touch_flash(void)
{
sim_flash_t sim;
gps_ephemeris_store_flash_t flash;
gps_nav_context_store_t store;
gps_nav_context_payload_t payload;
sim_init(&sim);
bind(&flash, &sim);
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
fill_payload(&payload, 5u);
bool wrote = false;
CHECK(gps_nav_context_store_save(&store, &payload, &wrote), "first save");
CHECK(wrote, "first save writes");
uint32_t erases = sim.erase_count;
uint32_t programs = sim.program_count;
for(unsigned int i = 0u; i < 50u; i++)
{
wrote = true;
CHECK(gps_nav_context_store_save(&store, &payload, &wrote),
"repeat save succeeds");
CHECK(!wrote, "repeat save reports no write");
}
CHECK(sim.erase_count == erases, "repeat saves cause no erase");
CHECK(sim.program_count == programs, "repeat saves cause no program");
CHECK(store.skipped_writes == 50u, "skipped writes counted");
/* A genuine change must still be written. */
gps_nav_context_payload_t changed;
fill_payload(&changed, 9u);
CHECK(gps_nav_context_store_save(&store, &changed, &wrote), "changed save");
CHECK(wrote, "changed content writes");
CHECK(sim.erase_count == (erases + 1u), "changed save erases once");
printf(" unchanged saves never reach the flash (%u skipped, %u erases)\n",
store.skipped_writes, sim.erase_count);
}
static void test_alternates_sectors(void)
{
sim_flash_t sim;
gps_ephemeris_store_flash_t flash;
gps_nav_context_store_t store;
gps_nav_context_payload_t payload;
sim_init(&sim);
bind(&flash, &sim);
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
unsigned int previous = 0u;
for(unsigned int i = 0u; i < 4u; i++)
{
fill_payload(&payload, i + 1u);
bool wrote = false;
CHECK(gps_nav_context_store_save(&store, &payload, &wrote), "save");
CHECK(wrote, "distinct content writes");
if(i != 0u)
{
CHECK(store.active_sector != previous,
"each save lands in the other sector");
}
previous = store.active_sector;
}
printf(" A/B alternation ok\n");
}
/*
* Power loss during a save must never lose the copy that was already good.
* The header is programmed last, so a torn save leaves a sector that does not
* validate and mount falls back to the previous generation.
*/
static void test_power_loss_keeps_previous_generation(void)
{
sim_flash_t sim;
gps_ephemeris_store_flash_t flash;
gps_nav_context_store_t store;
gps_nav_context_payload_t first;
gps_nav_context_payload_t second;
gps_nav_context_payload_t read;
sim_init(&sim);
bind(&flash, &sim);
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
fill_payload(&first, 11u);
bool wrote = false;
CHECK(gps_nav_context_store_save(&store, &first, &wrote), "first save");
/* Cut the very next save short partway through its payload. */
sim.fail_after_programs = (int32_t)sim.program_count + 2;
sim.truncate_bytes = 7u;
fill_payload(&second, 22u);
(void)gps_nav_context_store_save(&store, &second, &wrote);
gps_nav_context_store_t remounted;
CHECK(gps_nav_context_store_mount(&remounted, &flash),
"remount after power loss");
CHECK(gps_nav_context_store_load(&remounted, &read),
"still holds a payload");
CHECK(memcmp(&first, &read, sizeof(first)) == 0,
"the torn save did not replace the good copy");
printf(" power loss preserves previous generation\n");
}
static void test_corrupt_payload_is_rejected(void)
{
sim_flash_t sim;
gps_ephemeris_store_flash_t flash;
gps_nav_context_store_t store;
gps_nav_context_payload_t payload;
gps_nav_context_payload_t read;
sim_init(&sim);
bind(&flash, &sim);
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
fill_payload(&payload, 4u);
bool wrote = false;
CHECK(gps_nav_context_store_save(&store, &payload, &wrote), "save");
/*
* Clear a bit in the payload; only bit clears are physically possible on
* NOR. Find a byte that actually has a bit set, otherwise the "corruption"
* is a no-op and the test passes for the wrong reason.
*/
bool corrupted = false;
for(uint32_t i = 256u; i < (256u + sizeof(payload)); i++)
{
if(sim.data[i] != 0x00u)
{
sim.data[i] &= (uint8_t)(sim.data[i] - 1u);
corrupted = true;
break;
}
}
CHECK(corrupted, "test actually corrupted a payload byte");
gps_nav_context_store_t remounted;
CHECK(gps_nav_context_store_mount(&remounted, &flash), "remount");
CHECK(!gps_nav_context_store_load(&remounted, &read),
"payload failing CRC is not served");
printf(" corrupt payload rejected\n");
}
static void test_format_clears(void)
{
sim_flash_t sim;
gps_ephemeris_store_flash_t flash;
gps_nav_context_store_t store;
gps_nav_context_payload_t payload;
sim_init(&sim);
bind(&flash, &sim);
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
fill_payload(&payload, 6u);
bool wrote = false;
CHECK(gps_nav_context_store_save(&store, &payload, &wrote), "save");
CHECK(gps_nav_context_store_format(&store), "format");
CHECK(!gps_nav_context_store_load(&store, &payload), "format clears");
printf(" format ok\n");
}
int main(void)
{
test_empty_mount_holds_nothing();
test_round_trip();
test_unchanged_save_does_not_touch_flash();
test_alternates_sectors();
test_power_loss_keeps_previous_generation();
test_corrupt_payload_is_rejected();
test_format_clears();
if(failures != 0)
{
fprintf(stderr, "%d check(s) failed\n", failures);
return 1;
}
printf("gps nav context store tests passed\n");
return 0;
}
soc/software/gps_system/tests/test_gps_pvt_consistency.c
+499
View File
@@ -0,0 +1,499 @@
/*
* PVT consistency under the things that actually happen on the board:
* satellites appearing and disappearing mid-epoch, a 32-bit sample counter
* that wraps every 262 seconds, coalesced interrupts, and a cold start whose
* flash data is older than the receiver has any way to know.
*
* The question each test answers is not "does it compute a position" but "can
* this produce a position that is wrong without saying so".
*/
#include <assert.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "gps/gps_ip_defines.h"
#include "gps/gps_acq_manager.h"
#include "gps/gps_almanac.h"
#include "gps/gps_ephemeris_cache.h"
#include "gps/gps_ephemeris_validity.h"
#include "gps/gps_nav_epoch.h"
#include "gps/gps_pps.h"
#include "gps/gps_pps_time_filter.h"
#include "gps/Navigation.h"
static int failures;
#define CHECK(condition, message) do { \
if(!(condition)) { \
fprintf(stderr, "FAIL: %s\n", message); \
failures++; \
} \
} while(0)
/* The receiver's own constants, so the test cannot drift from the firmware. */
#define SAMPLE_RATE_HZ GPS_RF_SAMPLE_RATE_HZ
#define SAMPLES_PER_MS (SAMPLE_RATE_HZ / 1000.0)
#define SYNC_WINDOW ((uint32_t)(SAMPLES_PER_MS * \
(double)GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS))
#define TOW_TOLERANCE GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC
/* ================================================================== */
/* 1. Cold start: what does the acquisition engine actually search? */
/* ================================================================== */
/*
* At power-on there is no receiver time - it arrives only once a satellite is
* tracked and its HOW decoded - so the almanac cannot be evaluated and the
* visibility model must stand aside completely. Anything less would mean a
* cold start searching a subset chosen by data it cannot yet validate.
*/
static void test_cold_start_searches_every_prn(void)
{
gps_acq_manager_t manager;
gps_acq_manager_config_t config = {
.retry_delay_ticks = GPS_ACQ_RETRY_DELAY_TICKS,
.hot_retry_delay_ticks = GPS_ACQ_HOT_RETRY_DELAY_TICKS,
.hot_window_ticks = GPS_ACQ_REACQ_PRIORITY_TICKS,
.max_hot_streak = GPS_ACQ_MAX_HOT_STREAK,
.hot_fast_attempts = GPS_ACQ_HOT_FAST_ATTEMPTS,
.unlikely_retry_delay_ticks = GPS_ACQ_UNLIKELY_RETRY_DELAY_TICKS
};
gps_acq_manager_init(&manager, &config);
CHECK(gps_acq_manager_search_mask(&manager) == 0xffffffffu,
"acquisition did not start with every PRN searchable");
for(unsigned int prn = 0u; prn < GPS_ACQ_MANAGER_PRN_COUNT; prn++)
{
CHECK(gps_acq_manager_enable_cold(&manager, prn), "enable cold");
}
/*
* gps_refresh_acq_visibility() forces the mask wide whenever receiver time
* is unknown. Reproduce that and confirm one full sweep really does reach
* all 32 PRNs before any of them is retried.
*/
gps_acq_manager_set_search_mask(&manager, 0xffffffffu);
uint32_t swept = 0u;
uint64_t now = 1u;
for(unsigned int i = 0u; i < GPS_ACQ_MANAGER_PRN_COUNT; i++)
{
unsigned int prn = 0u;
bool hot = false;
CHECK(gps_acq_manager_start_next(&manager, now, &prn, &hot),
"cold start had nothing to search");
CHECK((swept & (1u << prn)) == 0u,
"a PRN was searched twice before the sweep completed");
swept |= 1u << prn;
gps_acq_manager_mark_failed(&manager, prn, now);
now += GPS_ACQ_RETRY_DELAY_TICKS;
}
CHECK(swept == 0xffffffffu, "the cold-start sweep missed a PRN");
printf(" cold start with no time: all 32 PRNs searched, none twice\n");
/*
* And an empty visibility model - which is what a corrupt or absent flash
* almanac produces - must degrade to the same blind sweep, never to
* silence.
*/
gps_acq_manager_set_search_mask(&manager, 0u);
CHECK(gps_acq_manager_search_mask(&manager) == 0xffffffffu,
"an empty visibility mask silenced acquisition");
printf(" empty visibility model degrades to a blind sweep\n");
}
/*
* The other half of the cold-start question: a flash ephemeris is installed
* while time is unknown, so what stops a months-old orbit reaching the
* position solve?
*
* gps_collect_completed_epoch_locked() qualifies every channel with
* time_known = true against the epoch's own transmit time, which comes from
* the decoded HOW and not from anything persisted. So the age check is
* unavoidable by the time a measurement can influence a fix, however the
* ephemeris got into the cache.
*/
static void test_stale_ephemeris_cannot_reach_the_solve(void)
{
gps_ephemeris_t eph;
memset(&eph, 0, sizeof(eph));
eph.ephemeris_valid = true;
eph.SVID = 5u;
eph.iode = 0x30u;
eph.iodc = 0x30u;
eph.toe.sec = 100000.0;
eph.toc.sec = 100000.0;
eph.toe.week = 2431;
eph.toe_week_valid = true;
eph.A = 26560000.0;
eph.sqrta = 5153.6;
eph.ecc = 0.006;
eph.sva = 2u;
eph.fit_flag = false; /* nominal four hour curve fit */
/* While time is unknown only the structural rules can apply - this is the
* cold-start window, and it is why the epoch gate exists. */
CHECK(gps_ephemeris_check(&eph, 5u, false, 0, 0.0) == GPS_EPHEMERIS_OK,
"structural check rejected a well-formed record");
/* Inside the fit interval: usable. */
CHECK(gps_ephemeris_check(&eph, 5u, true, 2431, 103000.0) ==
GPS_EPHEMERIS_OK,
"a record 50 minutes old was rejected");
/* Past it: refused, whatever it claims about itself. */
CHECK(gps_ephemeris_check(&eph, 5u, true, 2431, 100000.0 + 8000.0) ==
GPS_EPHEMERIS_REJECT_AGE,
"a record past its curve fit interval was accepted");
/* A week later, expressed as a wrapped second-of-week - the shape a
* flash record takes after a long power-off. */
CHECK(gps_ephemeris_check(&eph, 5u, true, 2432, 100000.0) ==
GPS_EPHEMERIS_REJECT_AGE,
"a week-old record was accepted");
printf(" restored ephemeris is age-checked before it can reach a fix\n");
}
/* ================================================================== */
/* 2. The 32-bit sample counter wraps every 262 s. */
/* ================================================================== */
/*
* Pseudoranges are differences of corrected sample counts against one
* reference, scaled to metres. The counter is 32 bits at 16.368 MHz, so it
* wraps mid-flight roughly every four and a half minutes. If the wrap leaked
* into the differences it would put one satellite 2^31 samples - about 39
* million kilometres - away from the rest, and the resulting position would be
* wrong with a perfectly ordinary-looking residual on the other channels.
*
* Build one epoch twice, once away from the wrap and once straddling it, and
* require the metre-domain pseudoranges to be identical.
*/
static void test_sample_wrap_does_not_disturb_pseudoranges(void)
{
/* Six channels spread over 20 ms of capture skew, with sub-sample code
* phase corrections, which is the realistic shape of one epoch. */
const int32_t offsets[6] = {0, -12345, 48000, -230000, 91000, 150000};
const double code_phase[6] = {0.31, 0.77, 0.02, 0.55, 0.94, 0.18};
double away[6];
double across[6];
for(unsigned int pass = 0u; pass < 2u; pass++)
{
/* Pass 0 sits mid-range; pass 1 puts the reference just past the
* wrap so half the channels are still near 0xFFFFFFFF. */
uint32_t base = (pass == 0u) ? 0x40000000u : 0x00000064u;
uint32_t reference = base;
double lifted[6];
for(unsigned int i = 0u; i < 6u; i++)
{
uint32_t raw = (uint32_t)((int64_t)base + offsets[i]);
double corrected = (double)raw - code_phase[i];
lifted[i] = gps_nav_epoch_unwrap_sample(raw, corrected,
reference);
}
/* Exactly what gps_ip.c forms: milliseconds relative to the
* reference, then metres. */
for(unsigned int i = 0u; i < 6u; i++)
{
double delta_ms = (lifted[i] / SAMPLES_PER_MS) -
(lifted[0] / SAMPLES_PER_MS);
double metres = delta_ms * (SPEED_OF_LIGHT / 1000.0);
if(pass == 0u) { away[i] = metres; } else { across[i] = metres; }
}
}
double worst = 0.0;
for(unsigned int i = 0u; i < 6u; i++)
{
double difference = fabs(away[i] - across[i]);
if(difference > worst)
{
worst = difference;
}
}
printf(" sample counter wrap: worst pseudorange difference %.9f m\n",
worst);
CHECK(worst < 1.0e-3, "the 32-bit sample wrap moved a pseudorange");
/* The epoch selector's own comparisons must agree across the wrap too,
* otherwise a coherent epoch would be rejected once every 262 s. */
/* Sized to the API's declared parameter, not to the count in use: the
* signature promises GPS_NAV_EPOCH_MAX_CANDIDATES entries even though it
* only reads candidate_count of them. */
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES];
memset(candidates, 0, sizeof(candidates));
gps_nav_epoch_selection_t selection;
for(unsigned int i = 0u; i < 6u; i++)
{
candidates[i].week = 2431;
candidates[i].tow_sec = 300000.0;
candidates[i].raw_sample =
(uint32_t)((int64_t)0x00000064 + offsets[i]);
}
CHECK(gps_nav_epoch_validate_complete(candidates, 6u, SYNC_WINDOW,
TOW_TOLERANCE, &selection),
"a coherent epoch straddling the counter wrap was rejected");
CHECK(selection.selected_count == 6u, "epoch lost a channel at the wrap");
printf(" epoch selection is wrap-safe\n");
}
/* ================================================================== */
/* 3. Satellites connecting and disconnecting. */
/* ================================================================== */
#define PERIOD_TICKS ((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_SOLVE_PERIOD_MS / 1000u)
#define WINDOW_TICKS (((uint64_t)GPS_PPS_CLOCK_HZ * \
GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS) / 1000u)
#define HOLDOFF_TICKS ((uint64_t)1000000)
static gps_nav_epoch_event_result_t push(gps_nav_epoch_assembly_t* assembly,
uint64_t counter,
unsigned int channel,
uint32_t ready_mask)
{
return gps_nav_epoch_assembly_push(assembly, counter, counter, channel,
ready_mask, PERIOD_TICKS,
WINDOW_TICKS, HOLDOFF_TICKS);
}
/*
* An epoch is an all-or-nothing hardware measurement set: the expected channel
* set is frozen when the first event of a period arrives, so the satellite
* geometry of a published epoch cannot depend on interrupt timing. These are
* the transitions that have to hold that line.
*/
static void test_epoch_assembly_under_connect_and_disconnect(void)
{
gps_nav_epoch_assembly_t assembly;
uint64_t counter = 1000000u;
/* --- a satellite acquired mid-epoch must wait for the next one --- */
gps_nav_epoch_assembly_reset(&assembly);
CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
"first event rejected");
CHECK(push(&assembly, counter + 10u, 1u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED, "second event rejected");
/* Channel 4 has just locked, so the live ready mask has grown. */
CHECK(push(&assembly, counter + 20u, 4u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_INVALID,
"a channel that joined mid-epoch was admitted to it");
CHECK(push(&assembly, counter + 30u, 2u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED, "third event rejected");
CHECK(push(&assembly, counter + 40u, 3u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_COMPLETE,
"the frozen expected set did not complete");
printf(" a satellite acquired mid-epoch joins the next epoch, not this one\n");
/* --- a satellite lost mid-epoch must release the epoch --- */
gps_nav_epoch_assembly_reset(&assembly);
CHECK(push(&assembly, counter, 0u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
"push 0");
CHECK(push(&assembly, counter + 5u, 1u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 1");
CHECK(push(&assembly, counter + 8u, 2u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 2");
CHECK(push(&assembly, counter + 9u, 3u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 3");
/* Channel 4 drops before it ever reports. Removing it must complete the
* epoch from what has already arrived rather than stall until the
* deadline. */
CHECK(gps_nav_epoch_assembly_remove_channel(&assembly, 4u),
"removing a lost channel did not complete the epoch");
CHECK(gps_nav_epoch_assembly_usable(&assembly,
GPS_NAV_MIN_WLS_SATELLITES),
"four surviving channels were not usable");
printf(" a satellite lost mid-epoch releases the epoch immediately\n");
/* --- losing one too many must not publish a short epoch --- */
gps_nav_epoch_assembly_reset(&assembly);
CHECK(push(&assembly, counter, 0u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
"push 0");
CHECK(push(&assembly, counter + 5u, 1u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 1");
CHECK(push(&assembly, counter + 8u, 2u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 2");
(void)gps_nav_epoch_assembly_remove_channel(&assembly, 3u);
(void)gps_nav_epoch_assembly_remove_channel(&assembly, 4u);
CHECK(!gps_nav_epoch_assembly_usable(&assembly,
GPS_NAV_MIN_WLS_SATELLITES),
"three channels were reported usable for a four-unknown solve");
printf(" three surviving channels are never offered as a fix\n");
/* --- every satellite lost --- */
gps_nav_epoch_assembly_reset(&assembly);
CHECK(push(&assembly, counter, 0u, 0x03u) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
"push 0");
CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 0u),
"removing the only channel reported completion");
CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 1u),
"removing the last channel reported completion");
CHECK(!gps_nav_epoch_assembly_usable(&assembly, 1u),
"an emptied assembly was still usable");
CHECK(!gps_nav_epoch_assembly_complete(&assembly),
"an emptied assembly reported complete");
printf(" total loss leaves no assembly behind\n");
/* --- the newest period supersedes an incomplete older one --- */
gps_nav_epoch_assembly_reset(&assembly);
CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
"push 0");
CHECK(push(&assembly, counter + 5u, 1u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 1");
CHECK(push(&assembly, counter + PERIOD_TICKS, 2u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_RESTARTED,
"an event from the next period did not restart the epoch");
printf(" a new capture period discards the incomplete previous one\n");
/* --- a straggler from the previous period is not back-filled --- */
CHECK(push(&assembly, counter, 3u, 0x0fu) == GPS_NAV_EPOCH_EVENT_STALE,
"an event from the previous period was folded into the new epoch");
printf(" a late event from the previous period is dropped, not mixed in\n");
/* --- a coalesced repeat must not be counted twice --- */
gps_nav_epoch_assembly_reset(&assembly);
CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
"push 0");
CHECK(push(&assembly, counter + 1u, 0u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_DUPLICATE,
"a repeated event for one channel was accepted twice");
printf(" a coalesced repeat is rejected as a duplicate\n");
/* --- an event neither in this epoch nor the next is an outlier --- */
gps_nav_epoch_assembly_reset(&assembly);
CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
"push 0");
CHECK(push(&assembly, counter + WINDOW_TICKS + 1u, 1u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_OUTLIER,
"an event outside the coherence window was accepted");
printf(" an out-of-window capture is refused, not averaged in\n");
}
/*
* Coalescing: the capture mailbox holds only the newest measurement, so when
* interrupts are late the receiver has to know how many epochs it skipped and
* must never invent the missing ones.
*/
static void test_coalesced_captures_are_counted_not_invented(void)
{
uint32_t gap = 0u;
CHECK(gps_nav_capture_sequence_consistent(1000u, 5u,
1000u + PERIOD_TICKS, 6u,
PERIOD_TICKS, &gap),
"a single clean epoch was rejected");
CHECK(gap == 1u, "single epoch reported a gap");
CHECK(gps_nav_capture_sequence_consistent(1000u, 5u,
1000u + (3u * PERIOD_TICKS), 8u,
PERIOD_TICKS, &gap),
"three coalesced epochs were rejected");
CHECK(gap == 3u, "coalesced gap miscounted");
/* Counter and sequence disagreeing means the mailbox cannot be trusted. */
CHECK(!gps_nav_capture_sequence_consistent(1000u, 5u,
1000u + (3u * PERIOD_TICKS), 6u,
PERIOD_TICKS, &gap),
"a sequence/counter mismatch was accepted");
CHECK(!gps_nav_capture_sequence_consistent(1000u, 5u, 1000u, 6u,
PERIOD_TICKS, &gap),
"a counter that did not advance was accepted");
CHECK(!gps_nav_capture_sequence_consistent(0u, 5u,
1000u + PERIOD_TICKS, 6u,
PERIOD_TICKS, &gap),
"an unseeded previous counter was accepted");
printf(" coalesced captures are counted exactly, never synthesised\n");
}
/*
* Publication is stricter than clustering: every channel in the frozen ready
* mask must belong to one coherent epoch, or nothing is published. A partial
* cluster would change the satellite geometry according to interrupt timing,
* which is exactly what produced large raw-position jumps before.
*/
static void test_partial_coherence_publishes_nothing(void)
{
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES];
memset(candidates, 0, sizeof(candidates));
gps_nav_epoch_selection_t selection;
for(unsigned int i = 0u; i < 5u; i++)
{
candidates[i].week = 2431;
candidates[i].tow_sec = 300000.0;
candidates[i].raw_sample = 1000000u + (i * 100u);
}
CHECK(gps_nav_epoch_validate_complete(candidates, 5u, SYNC_WINDOW,
TOW_TOLERANCE, &selection),
"a fully coherent set was rejected");
/* One channel a full nav period away - a stale measurement that slipped
* through. Four good ones remain, which is enough for a solve, and that
* is exactly why it must not be published as one. */
candidates[4].raw_sample = 1000000u + (uint32_t)(SAMPLES_PER_MS * 200.0);
CHECK(!gps_nav_epoch_validate_complete(candidates, 5u, SYNC_WINDOW,
TOW_TOLERANCE, &selection),
"an incoherent epoch was published because a subset was solvable");
CHECK(selection.selected_count == 4u,
"the diagnostic cluster did not report the coherent subset");
/* A TOW disagreement is equally disqualifying even when the samples line
* up: it means two satellites disagree about what time it is. */
candidates[4].raw_sample = 1000000u + 400u;
candidates[4].tow_sec = 300000.0 + (10.0 * TOW_TOLERANCE);
CHECK(!gps_nav_epoch_validate_complete(candidates, 5u, SYNC_WINDOW,
TOW_TOLERANCE, &selection),
"channels disagreeing about TOW were published as one epoch");
printf(" an incoherent epoch publishes nothing, not its solvable subset\n");
}
/*
* A non-finite transmit time must never reach the pseudorange arithmetic.
* gps_pps_select_reference() is the last gate before it, and it screens the
* whole set - not just the entries it happens to walk past before finding the
* preferred reference satellite.
*/
static void test_reference_selection_screens_every_capture(void)
{
unsigned int ids[4] = {3u, 7u, 11u, 19u};
double times[4] = {10.0, 11.0, 12.0, 13.0};
unsigned int reference = 99u;
CHECK(gps_pps_select_reference(ids, times, 4u, true, 7u, &reference),
"a clean set was rejected");
CHECK(reference == 1u, "the preferred reference satellite was not chosen");
CHECK(gps_pps_select_reference(ids, times, 4u, false, 0u, &reference),
"a clean set without a preference was rejected");
CHECK(reference == 0u, "the earliest capture was not chosen");
/* The corrupt entry sits *after* the preferred satellite, which is the
* ordering an early return would walk straight past. */
times[3] = NAN;
CHECK(!gps_pps_select_reference(ids, times, 4u, true, 7u, &reference),
"a non-finite capture time downstream of the preferred reference "
"was not detected");
CHECK(!gps_pps_select_reference(ids, times, 4u, false, 0u, &reference),
"a non-finite capture time was not detected without a preference");
printf(" reference selection screens the whole capture set\n");
}
int main(void)
{
test_cold_start_searches_every_prn();
test_stale_ephemeris_cannot_reach_the_solve();
test_sample_wrap_does_not_disturb_pseudoranges();
test_epoch_assembly_under_connect_and_disconnect();
test_coalesced_captures_are_counted_not_invented();
test_partial_coherence_publishes_nothing();
test_reference_selection_screens_every_capture();
if(failures != 0)
{
fprintf(stderr, "%d check(s) failed\n", failures);
return 1;
}
printf("gps pvt consistency tests passed\n");
return 0;
}
soc/software/gps_system/tests/test_gps_time_anchor.c
+217
View File
@@ -183,8 +183,225 @@ static void fractional_code_phase_corrects_capture_counter(void)
"capture correction beyond llround range was accepted");
}
#define PPS_HZ 125000000.0
#define BIT_PERIOD_SEC 0.020
#define NAV_TICKS 10u
#define GUARD_SEC 0.004
#define SNAP_SEC 0.050
/*
* These exercise the two functions the firmware actually calls when a returning
* PRN is warm-seeded: the phase handed to NAV_SOLVE_PHASE_SEED, and the snap
* that turns the first seeded epoch into a time anchor.
*
* Contract under test (verified against the RTL in
* hardware/external/gps/rtl/gps_timer/tb/nav_solve_timer_seed_tb.sv): the seed
* is the counter value in effect for the NEXT navigation bit, and an epoch is
* raised on absolute bit index b with (b + 1) % NAV_TICKS == 0.
*/
/* Place the receiver a known fraction into a navigation bit so the guard band
* is not what is being measured. */
static double elapsed_for_outage(double outage_sec, double into_bit_sec)
{
double bits = floor(outage_sec / BIT_PERIOD_SEC);
return (bits * BIT_PERIOD_SEC) + into_bit_sec;
}
static void phase_seed_matches_the_global_bit_grid(void)
{
const double anchor_tow = 469362.0; /* a HOW boundary: multiple of 6 s */
const double outages[] = {0.020, 0.100, 0.200, 1.0, 5.0, 30.0, 60.0,
300.0, 900.0};
for(unsigned int i = 0u;
i < (sizeof(outages) / sizeof(outages[0])); i++)
{
uint32_t phase = 0u;
double elapsed = elapsed_for_outage(outages[i], BIT_PERIOD_SEC / 2.0);
if(!gps_nav_bit_phase_from_elapsed(anchor_tow, elapsed,
BIT_PERIOD_SEC, NAV_TICKS,
GUARD_SEC, &phase))
{
fprintf(stderr, "FAIL: seed refused for a %.3f s outage\n",
outages[i]);
failures++;
continue;
}
/* The next bit after the one the receiver is currently inside. */
double predicted = anchor_tow + elapsed;
uint64_t next_index =
(uint64_t)floor(predicted / BIT_PERIOD_SEC) + 1u;
CHECK(phase == (uint32_t)(next_index % NAV_TICKS),
"seed phase does not match the global navigation-bit grid");
/* Applying that phase, the hardware counts up to NAV_TICKS-1; the
* epoch therefore lands where the uninterrupted grid would have. */
uint64_t epoch_index = next_index + (NAV_TICKS - 1u - phase);
CHECK(((epoch_index + 1u) % NAV_TICKS) == 0u,
"seeded epoch would land off the global grid");
}
}
static void phase_seed_guard_band_rejects_bit_boundaries(void)
{
const double anchor_tow = 469362.0;
uint32_t phase = 0u;
/* Just after a boundary. */
CHECK(!gps_nav_bit_phase_from_elapsed(
anchor_tow, elapsed_for_outage(10.0, GUARD_SEC / 2.0),
BIT_PERIOD_SEC, NAV_TICKS, GUARD_SEC, &phase),
"arming just after a navigation-bit boundary was allowed");
/* Just before the next one. */
CHECK(!gps_nav_bit_phase_from_elapsed(
anchor_tow,
elapsed_for_outage(10.0, BIT_PERIOD_SEC - (GUARD_SEC / 2.0)),
BIT_PERIOD_SEC, NAV_TICKS, GUARD_SEC, &phase),
"arming just before a navigation-bit boundary was allowed");
/* Comfortably inside is fine. */
CHECK(gps_nav_bit_phase_from_elapsed(
anchor_tow, elapsed_for_outage(10.0, BIT_PERIOD_SEC / 2.0),
BIT_PERIOD_SEC, NAV_TICKS, GUARD_SEC, &phase),
"arming mid-bit was refused");
}
static void phase_seed_is_always_reduced(void)
{
/* The tracking IP refuses an unreduced seed, so software must never emit
* one for any outage length or anchor. */
const double anchors[] = {0.0, 6.0, 302400.0, 604794.0};
for(unsigned int a = 0u; a < (sizeof(anchors) / sizeof(anchors[0])); a++)
{
for(double outage = 0.02; outage < 900.0; outage *= 1.7)
{
uint32_t phase = 0u;
double elapsed =
elapsed_for_outage(outage, BIT_PERIOD_SEC / 2.0);
if(gps_nav_bit_phase_from_elapsed(anchors[a], elapsed,
BIT_PERIOD_SEC, NAV_TICKS,
GUARD_SEC, &phase))
{
CHECK(phase < NAV_TICKS, "emitted seed is not reduced");
}
}
}
}
static void phase_seed_rejects_invalid_requests(void)
{
uint32_t phase = 0u;
CHECK(!gps_nav_bit_phase_from_elapsed(469362.0, -1.0, BIT_PERIOD_SEC,
NAV_TICKS, GUARD_SEC, &phase),
"negative elapsed time was accepted");
CHECK(!gps_nav_bit_phase_from_elapsed(469362.0, 10.0, BIT_PERIOD_SEC,
0u, GUARD_SEC, &phase),
"a zero nav-solve multiplier was accepted");
CHECK(!gps_nav_bit_phase_from_elapsed(-1.0, 10.0, BIT_PERIOD_SEC,
NAV_TICKS, GUARD_SEC, &phase),
"a negative anchor time of week was accepted");
CHECK(!gps_nav_bit_phase_from_elapsed(469362.0, 10.0, BIT_PERIOD_SEC,
NAV_TICKS, BIT_PERIOD_SEC, &phase),
"a guard band wider than half a bit was accepted");
}
static void epoch_snap_recovers_transmit_time(void)
{
const double period = (double)NAV_TICKS * BIT_PERIOD_SEC; /* 0.2 s */
const double anchor_tow = 469362.0;
double snapped = 0.0;
/* An exact multiple of the epoch period comes back unchanged. */
CHECK(gps_nav_epoch_snap_from_elapsed(anchor_tow, 60.0, period,
SNAP_SEC, &snapped),
"an exact epoch was refused");
CHECK(fabs(snapped - (anchor_tow + 60.0)) < 1e-9,
"an exact epoch was moved by the snap");
/* Prediction error inside the tolerance is removed entirely: this is what
* turns a drifted prediction back into exact GPS time. */
CHECK(gps_nav_epoch_snap_from_elapsed(anchor_tow, 60.0 + 0.03, period,
SNAP_SEC, &snapped),
"a prediction inside the snap tolerance was refused");
CHECK(fabs(snapped - (anchor_tow + 60.0)) < 1e-9,
"the snap did not remove the prediction error");
CHECK(gps_nav_epoch_snap_from_elapsed(anchor_tow, 60.0 - 0.03, period,
SNAP_SEC, &snapped),
"a negative prediction error inside tolerance was refused");
CHECK(fabs(snapped - (anchor_tow + 60.0)) < 1e-9,
"the snap did not remove a negative prediction error");
/* Beyond the tolerance the context is declared stale rather than snapped
* to the wrong epoch. */
CHECK(!gps_nav_epoch_snap_from_elapsed(anchor_tow, 60.0 + 0.08, period,
SNAP_SEC, &snapped),
"a prediction outside the snap tolerance was accepted");
/* The result may cross a week boundary; the caller normalises. */
CHECK(gps_nav_epoch_snap_from_elapsed(604799.8, 0.4, period,
SNAP_SEC, &snapped),
"an epoch crossing the week boundary was refused");
CHECK(snapped > 604800.0,
"week crossing was silently wrapped inside the snap");
CHECK(!gps_nav_epoch_snap_from_elapsed(anchor_tow, 60.0, period,
period, &snapped),
"a snap tolerance of half a period or more was accepted");
}
/*
* Receiver-clock drift is what ultimately bounds warm-seed reuse. The seed
* only needs to land in the right 20 ms bit, and the epoch snap only needs to
* land in the right 200 ms epoch, so both tolerate far more drift than the
* ephemeris fit interval allows.
*/
static void drift_bounds_are_where_they_are_claimed(void)
{
const double period = (double)NAV_TICKS * BIT_PERIOD_SEC;
const double anchor_tow = 400000.0;
const double outage = 900.0; /* the configured reuse limit */
uint32_t phase = 0u;
double snapped = 0.0;
/* At 2 ppm over 15 minutes the error is 1.8 ms: both stages cope. */
double drift_2ppm = outage * 2e-6;
CHECK(gps_nav_epoch_snap_from_elapsed(anchor_tow, outage + drift_2ppm,
period, SNAP_SEC, &snapped),
"2 ppm over 15 minutes broke the epoch snap");
CHECK(fabs(snapped - (anchor_tow + outage)) < 1e-9,
"2 ppm drift was not removed by the snap");
/* The snap fails once drift exceeds half an epoch period; at 15 minutes
* that needs well over 50 ppm, far beyond any plausible oscillator. */
double drift_100ppm = outage * 100e-6; /* 90 ms */
CHECK(!gps_nav_epoch_snap_from_elapsed(anchor_tow, outage + drift_100ppm,
period, SNAP_SEC, &snapped),
"100 ppm over 15 minutes was accepted by the epoch snap");
/* The phase seed is the tighter of the two: it must land in the correct
* 20 ms bit, so a drift of one whole bit shifts the grid. */
double elapsed_ok = elapsed_for_outage(outage, BIT_PERIOD_SEC / 2.0);
CHECK(gps_nav_bit_phase_from_elapsed(anchor_tow, elapsed_ok,
BIT_PERIOD_SEC, NAV_TICKS,
GUARD_SEC, &phase),
"the nominal 15 minute seed was refused");
uint32_t shifted = 0u;
CHECK(gps_nav_bit_phase_from_elapsed(anchor_tow,
elapsed_ok + BIT_PERIOD_SEC,
BIT_PERIOD_SEC, NAV_TICKS,
GUARD_SEC, &shifted),
"a one-bit shifted seed was refused");
CHECK(shifted == ((phase + 1u) % NAV_TICKS),
"a one-bit prediction error does not shift the phase by one");
/* That is the failure this cannot detect on its own, which is why the
* epoch snap and the hardware's subframe override both exist. */
}
int main(void)
{
phase_seed_matches_the_global_bit_grid();
phase_seed_guard_band_rejects_bit_boundaries();
phase_seed_is_always_reduced();
phase_seed_rejects_invalid_requests();
epoch_snap_recovers_transmit_time();
drift_bounds_are_where_they_are_claimed();
normal_and_skipped_subframes_are_consistent();
week_rollover_is_consistent();
temporary_week_can_upgrade_on_first_subframe_one();
soc/software/gps_system/tests/test_gps_warm_reacquisition_sim.c
+736
View File
@@ -0,0 +1,736 @@
/*
* Host model of the warm/hot reacquisition path.
*
* This closes the gap between the two verification layers that already exist:
* Questa proves the RTL honours the seed contract, and the unit tests prove the
* firmware maths is self-consistent. Neither shows that the two AGREE. Here a
* behavioural model of nav_solve_timer.sv is driven by the real firmware
* functions (gps_time_anchor.c, gps_nav_epoch.c), so a disagreement about the
* epoch grid fails the build rather than the board.
*
* The timer model is written from the RTL contract, not copied from the
* firmware, so it is an independent check. Every behaviour it implements is
* covered by a matching case in
* hardware/external/gps/rtl/gps_timer/tb/nav_solve_timer_seed_tb.sv.
*/
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "gps/gps_nav_epoch.h"
#include "gps/gps_time_anchor.h"
#define NAV_TICKS 10u /* the configured multiplier */
#define BIT_PERIOD_SEC 0.020
#define EPOCH_PERIOD_SEC ((double)NAV_TICKS * BIT_PERIOD_SEC) /* 200 ms */
#define GUARD_SEC 0.004
#define SNAP_SEC 0.050
#define PPS_HZ 125000000.0
#define MAX_REUSE_SEC 900.0
static int failures;
#define CHECK(condition, message) do { \
if(!(condition)) { \
fprintf(stderr, "FAIL: %s\n", message); \
failures++; \
} \
} while(0)
/* ------------------------------------------------------------------ */
/* Behavioural model of nav_solve_timer.sv */
/* ------------------------------------------------------------------ */
typedef enum { NST_IDLE = 0, NST_COUNT } nst_state_t;
typedef struct {
nst_state_t state;
uint32_t counter;
uint32_t max;
bool provisional;
bool enable;
bool busy;
} nst_model_t;
static void nst_init(nst_model_t* m, uint32_t max)
{
memset(m, 0, sizeof(*m));
m->max = max;
}
static void nst_set_busy(nst_model_t* m, bool busy)
{
m->busy = busy;
if(!busy)
{
/* trck_busy deasserting returns the timer to IDLE and clears the
* provisional flag. */
m->state = NST_IDLE;
m->counter = 0u;
m->provisional = false;
}
}
/*
* A decoded subframe always wins: it aligns from IDLE, and it also re-aligns a
* channel that is counting on an unconfirmed seed. z_count is the HOW Z-count,
* so the first bit after the boundary has absolute index z_count * 300.
*/
static void nst_subframe(nst_model_t* m, uint64_t z_count)
{
if(!m->busy || !m->enable || (m->max == 0u))
{
return;
}
if((m->state == NST_IDLE) || m->provisional)
{
m->counter = (uint32_t)((z_count * 300u) % m->max);
m->provisional = false;
m->state = NST_COUNT;
}
}
/* Returns true when the seed was taken. The arm is always consumed. */
static bool nst_seed(nst_model_t* m, uint32_t phase)
{
if((m->state != NST_IDLE) || !m->busy || !m->enable ||
(m->max == 0u) || (phase >= m->max))
{
return false;
}
m->counter = phase;
m->provisional = true;
m->state = NST_COUNT;
return true;
}
/* One navigation bit. Returns true when this bit raises a nav-solve epoch. */
static bool nst_bit(nst_model_t* m)
{
if((m->state != NST_COUNT) || !m->enable || !m->busy)
{
return false;
}
bool epoch = (m->counter == (m->max - 1u));
m->counter = epoch ? 0u : (m->counter + 1u);
return epoch;
}
/* ------------------------------------------------------------------ */
/* Receiver model */
/* ------------------------------------------------------------------ */
typedef struct {
double tow_sec; /* transmit time of the last confirmed epoch */
uint64_t pps_counter; /* PPS capture at that epoch */
bool valid;
} prn_context_t;
/* Absolute navigation-bit index of an exact bit boundary. */
static uint64_t bit_index_of(double tow_sec)
{
return (uint64_t)llround(tow_sec / BIT_PERIOD_SEC);
}
/* Index of the bit the receiver is currently inside, for an arbitrary time. */
static uint64_t current_bit_index(double tow_sec)
{
return (uint64_t)floor(tow_sec / BIT_PERIOD_SEC);
}
/* The grid the receiver must reproduce: epochs on bits where (b+1)%N == 0. */
static bool is_epoch_bit(uint64_t bit_index)
{
return ((bit_index + 1u) % NAV_TICKS) == 0u;
}
/*
* A stored PRN context always holds the transmit time of a previous nav-solve
* epoch, so it necessarily sits on the epoch grid. That matters: epoch times
* are NOT multiples of the epoch period in time of week (with N=10 they fall at
* 0.18 s modulo 0.2 s), so anything that recovers an epoch time has to work in
* elapsed time relative to another epoch, never in absolute time of week.
*/
static double epoch_tow_at_or_below(double tow_sec)
{
uint64_t b = current_bit_index(tow_sec);
while(!is_epoch_bit(b))
{
b--;
}
return (double)b * BIT_PERIOD_SEC;
}
static uint64_t pps_at(double tow_sec, double tow_origin,
uint64_t pps_origin, double clock_ppm)
{
double elapsed = (tow_sec - tow_origin) * (1.0 + (clock_ppm * 1e-6));
return pps_origin + (uint64_t)llround(elapsed * PPS_HZ);
}
/*
* Drive one reacquisition: the PRN dropped after an epoch at context->tow_sec,
* came back outage_sec later, and firmware seeds the timer. Returns the number
* of bits fed after the seed, and asserts that every epoch raised lands on the
* uninterrupted global grid.
*/
static void run_reacquisition(double outage_sec,
double clock_ppm,
double into_bit_frac,
bool expect_seeded)
{
/* The context is a previous nav-solve epoch, not an arbitrary time. */
const double anchor_tow = epoch_tow_at_or_below(469362.0);
const uint64_t anchor_pps = 1000000000ull;
prn_context_t context = {
.tow_sec = anchor_tow, .pps_counter = anchor_pps, .valid = true
};
nst_model_t timer;
nst_init(&timer, NAV_TICKS);
timer.enable = true;
nst_set_busy(&timer, true);
/* Where the receiver actually is when firmware gets round to arming. */
double bits = floor(outage_sec / BIT_PERIOD_SEC);
double true_now_tow =
anchor_tow + (bits * BIT_PERIOD_SEC) +
(into_bit_frac * BIT_PERIOD_SEC);
uint64_t now_pps =
pps_at(true_now_tow, anchor_tow, anchor_pps, clock_ppm);
/* Firmware: elapsed from the PPS domain, then the seed. */
double elapsed_sec =
(double)(now_pps - context.pps_counter) / PPS_HZ;
uint32_t phase = 0u;
bool armed = gps_nav_bit_phase_from_elapsed(
context.tow_sec, elapsed_sec, BIT_PERIOD_SEC, NAV_TICKS,
GUARD_SEC, &phase);
if(!expect_seeded)
{
CHECK(!armed, "a seed was armed where the guard band should refuse");
return;
}
CHECK(armed, "firmware refused to arm a seed it should have accepted");
if(!armed)
{
return;
}
CHECK(phase < NAV_TICKS, "firmware emitted an unreduced seed");
CHECK(nst_seed(&timer, phase), "the tracking IP refused a valid seed");
/* Feed real navigation bits and check every epoch against the grid. */
uint64_t next_bit = current_bit_index(true_now_tow) + 1u;
unsigned int epochs = 0u;
bool first_epoch_seen = false;
double first_epoch_tow = 0.0;
for(unsigned int i = 0u; i < (4u * NAV_TICKS); i++)
{
uint64_t b = next_bit + i;
if(nst_bit(&timer))
{
epochs++;
CHECK(is_epoch_bit(b),
"a seeded epoch landed off the global navigation-bit grid");
if(!first_epoch_seen)
{
first_epoch_seen = true;
first_epoch_tow = (double)b * BIT_PERIOD_SEC;
}
}
}
CHECK(epochs >= 3u, "seeded channel did not produce periodic epochs");
CHECK(first_epoch_seen, "seeded channel produced no epoch at all");
/* Firmware then turns that first epoch into a time anchor via the snap. */
if(first_epoch_seen)
{
uint64_t epoch_pps =
pps_at(first_epoch_tow, anchor_tow, anchor_pps, clock_ppm);
double epoch_elapsed =
(double)(epoch_pps - context.pps_counter) / PPS_HZ;
double snapped = 0.0;
CHECK(gps_nav_epoch_snap_from_elapsed(
context.tow_sec, epoch_elapsed, EPOCH_PERIOD_SEC,
SNAP_SEC, &snapped),
"the first seeded epoch failed the anchor snap");
CHECK(fabs(snapped - first_epoch_tow) < 1e-6,
"the anchor snap recovered the wrong transmit time");
}
}
/* ------------------------------------------------------------------ */
static void outage_matrix_rejoins_on_the_correct_grid(void)
{
const double outages[] = {0.020, 0.100, 0.200, 1.0, 5.0, 30.0, 60.0,
300.0, 900.0};
for(unsigned int i = 0u;
i < (sizeof(outages) / sizeof(outages[0])); i++)
{
run_reacquisition(outages[i], 0.0, 0.5, true);
/* A realistic oscillator must not change the outcome. */
run_reacquisition(outages[i], 2.0, 0.5, true);
run_reacquisition(outages[i], -2.0, 0.5, true);
}
}
static void guard_band_refuses_arming_near_a_bit_edge(void)
{
/* Just inside either edge of the 20 ms bit: firmware must decline and
* retry rather than risk a one-bit grid shift. */
run_reacquisition(30.0, 0.0, 0.05, false);
run_reacquisition(30.0, 0.0, 0.95, false);
/* Mid-bit is fine. */
run_reacquisition(30.0, 0.0, 0.50, true);
}
static void seeded_grid_equals_the_uninterrupted_grid(void)
{
/* Run a channel that never dropped and one that dropped and was seeded,
* and require identical epoch bits. */
const double anchor_tow = epoch_tow_at_or_below(469362.0);
nst_model_t reference;
nst_init(&reference, NAV_TICKS);
reference.enable = true;
nst_set_busy(&reference, true);
uint64_t z = (uint64_t)(bit_index_of(anchor_tow) / 300u);
nst_subframe(&reference, z);
uint64_t reference_first_bit = z * 300u;
nst_model_t seeded;
nst_init(&seeded, NAV_TICKS);
seeded.enable = true;
nst_set_busy(&seeded, true);
uint32_t phase = (uint32_t)(reference_first_bit % NAV_TICKS);
CHECK(nst_seed(&seeded, phase), "seed refused for the comparison run");
for(unsigned int i = 0u; i < (5u * NAV_TICKS); i++)
{
bool a = nst_bit(&reference);
bool b = nst_bit(&seeded);
CHECK(a == b,
"seeded channel epochs diverged from the subframe-aligned channel");
if(a)
{
CHECK(is_epoch_bit(reference_first_bit + i),
"reference channel epoch is off the global grid");
}
}
}
static void a_wrong_seed_is_corrected_by_the_first_subframe(void)
{
const double anchor_tow = epoch_tow_at_or_below(469362.0);
uint64_t z = (uint64_t)(bit_index_of(anchor_tow) / 300u);
uint64_t first_bit = z * 300u;
nst_model_t timer;
nst_init(&timer, NAV_TICKS);
timer.enable = true;
nst_set_busy(&timer, true);
/* Seed deliberately three bits wrong, as a stale context would. */
uint32_t wrong = (uint32_t)((first_bit + 3u) % NAV_TICKS);
CHECK(nst_seed(&timer, wrong), "wrong seed refused");
CHECK(timer.provisional, "seeded channel is not marked provisional");
unsigned int off_grid = 0u;
for(unsigned int i = 0u; i < NAV_TICKS; i++)
{
if(nst_bit(&timer) && !is_epoch_bit(first_bit + i))
{
off_grid++;
}
}
CHECK(off_grid > 0u,
"the wrong-seed scenario did not actually produce a wrong grid");
/* The first decoded subframe re-aligns and clears provisional. */
uint64_t z2 = z + 1u;
nst_subframe(&timer, z2);
CHECK(!timer.provisional,
"provisional flag survived a decoded subframe");
uint64_t bit = z2 * 300u;
unsigned int epochs = 0u;
for(unsigned int i = 0u; i < (3u * NAV_TICKS); i++)
{
if(nst_bit(&timer))
{
epochs++;
CHECK(is_epoch_bit(bit + i),
"epoch still off-grid after the subframe override");
}
}
CHECK(epochs >= 2u, "no epochs after the subframe override");
}
static void a_stale_seed_cannot_survive_a_drop(void)
{
nst_model_t timer;
nst_init(&timer, NAV_TICKS);
timer.enable = true;
nst_set_busy(&timer, true);
CHECK(nst_seed(&timer, 2u), "seed refused");
/* Arming again while counting must be ignored. */
CHECK(!nst_seed(&timer, 5u), "a late seed overwrote a running phase");
nst_set_busy(&timer, false);
CHECK(!timer.provisional, "provisional survived a channel drop");
nst_set_busy(&timer, true);
unsigned int epochs = 0u;
for(unsigned int i = 0u; i < (4u * NAV_TICKS); i++)
{
if(nst_bit(&timer)) epochs++;
}
CHECK(epochs == 0u, "a stale seed restarted epochs after a drop");
}
static void stale_context_is_refused_by_the_snap(void)
{
/* A context so old that the receiver clock has drifted past half an epoch
* period must be refused rather than snapped onto the wrong epoch. */
const double anchor_tow = epoch_tow_at_or_below(469362.0);
double snapped = 0.0;
CHECK(!gps_nav_epoch_snap_from_elapsed(
anchor_tow, 300.0 + 0.09, EPOCH_PERIOD_SEC,
SNAP_SEC, &snapped),
"a context drifted most of an epoch was accepted");
/*
* Regression for a real defect: snapping the ABSOLUTE time of week instead
* of the elapsed time silently shifted every recovered epoch by up to half
* a period. An anchor that is deliberately not a multiple of the epoch
* period catches it; one that is cannot.
*/
CHECK(fmod(anchor_tow, EPOCH_PERIOD_SEC) > 1e-9,
"the regression anchor is a period multiple and proves nothing");
CHECK(gps_nav_epoch_snap_from_elapsed(anchor_tow, 12.0, EPOCH_PERIOD_SEC,
SNAP_SEC, &snapped),
"an exact epoch multiple was refused");
CHECK(fabs(snapped - (anchor_tow + 12.0)) < 1e-9,
"the snap moved an epoch that was already exact");
/* And a context beyond the reuse window is refused before that. */
double elapsed = MAX_REUSE_SEC + 1.0;
CHECK(elapsed > MAX_REUSE_SEC,
"reuse window constant is inconsistent with the firmware bound");
}
/*
* Epoch coalescing with a channel that joins mid-stream, using the real
* gps_nav_epoch.c assembly the firmware runs.
*/
static void a_seeded_channel_joins_a_live_coalescing_epoch(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
const uint64_t period = 25000000ull; /* 200 ms at 125 MHz */
const uint64_t window = 3000000ull;
const uint64_t holdoff = 4000000ull;
uint64_t counter = 5000000000ull;
uint64_t local = 1000ull;
/* Four established channels complete an epoch. */
uint32_t ready = 0x0fu;
gps_nav_epoch_event_result_t r = GPS_NAV_EPOCH_EVENT_INVALID;
for(unsigned int ch = 0u; ch < 4u; ch++)
{
r = gps_nav_epoch_assembly_push(&assembly, counter + ch, local + ch,
ch, ready, period, window, holdoff);
CHECK((r == GPS_NAV_EPOCH_EVENT_ACCEPTED) ||
(r == GPS_NAV_EPOCH_EVENT_COMPLETE),
"an established channel was not accepted into the epoch");
}
CHECK(r == GPS_NAV_EPOCH_EVENT_COMPLETE,
"a full expected set did not complete the epoch");
/* Next epoch: a seeded channel 4 is now in the ready mask. The epoch must
* still complete, now expecting five. */
gps_nav_epoch_assembly_reset(&assembly);
counter += period;
local += period;
ready = 0x1fu;
for(unsigned int ch = 0u; ch < 5u; ch++)
{
r = gps_nav_epoch_assembly_push(&assembly, counter + ch, local + ch,
ch, ready, period, window, holdoff);
}
CHECK(r == GPS_NAV_EPOCH_EVENT_COMPLETE,
"the epoch did not complete once a seeded channel joined");
/* A channel dropping mid-epoch must not deadlock the next one. */
gps_nav_epoch_assembly_reset(&assembly);
counter += period;
local += period;
for(unsigned int ch = 0u; ch < 3u; ch++)
{
(void)gps_nav_epoch_assembly_push(&assembly, counter + ch, local + ch,
ch, ready, period, window, holdoff);
}
CHECK(!gps_nav_epoch_assembly_complete(&assembly),
"epoch reported complete before its expected set arrived");
CHECK(gps_nav_epoch_assembly_remove_channel(&assembly, 3u) ||
!gps_nav_epoch_assembly_complete(&assembly),
"removing a dropped channel left the epoch inconsistent");
(void)gps_nav_epoch_assembly_remove_channel(&assembly, 4u);
CHECK(gps_nav_epoch_assembly_usable(&assembly, 3u),
"a three-satellite remainder was not usable after two drops");
/* A late event from the previous epoch must not contaminate this one. */
gps_nav_epoch_assembly_reset(&assembly);
counter += period;
local += period;
(void)gps_nav_epoch_assembly_push(&assembly, counter, local, 0u, ready,
period, window, holdoff);
r = gps_nav_epoch_assembly_push(&assembly, counter - period, local + 1u,
1u, ready, period, window, holdoff);
CHECK(r == GPS_NAV_EPOCH_EVENT_STALE,
"a previous-epoch event was not rejected as stale");
r = gps_nav_epoch_assembly_push(&assembly, counter, local + 2u, 0u, ready,
period, window, holdoff);
CHECK(r == GPS_NAV_EPOCH_EVENT_DUPLICATE,
"a duplicated channel event was not rejected");
}
/*
* Reproduces the failure observed on the board: a channel that is busy, locked
* and interrupt-enabled but whose hardware timer is idle emits nothing, yet is
* latched into expected_mask when the epoch opens. The epoch can then never
* complete and is discarded at its deadline.
*
* Run twice: with exclusion disabled it must lose every epoch (the defect), and
* with exclusion enabled only the first few (the fix). Without the disabled
* case this test would pass even if the fix did nothing.
*/
static void run_silent_channel_epochs(unsigned int limit,
unsigned int epochs,
unsigned int* completed_out,
unsigned int* discarded_out,
unsigned int* silent_miss_out)
{
const uint64_t period = 25000000ull; /* 200 ms at 125 MHz */
const uint64_t window = 3000000ull;
const uint64_t holdoff = 4000000ull;
const unsigned int SILENT = 3u;
unsigned int miss[8];
memset(miss, 0, sizeof(miss));
uint64_t counter = 5000000000ull;
uint64_t local = 1000ull;
unsigned int completed = 0u, discarded = 0u;
for(unsigned int epoch = 0u; epoch < epochs; epoch++)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
uint32_t ready = 0u;
for(unsigned int ch = 0u; ch < 4u; ch++)
{
/* limit == 0 models the original policy: never exclude. */
if((limit == 0u) || (miss[ch] < limit)) ready |= (1u << ch);
}
gps_nav_epoch_event_result_t r = GPS_NAV_EPOCH_EVENT_INVALID;
for(unsigned int ch = 0u; ch < 4u; ch++)
{
if(ch == SILENT) continue; /* emits nothing, ever */
if((ready & (1u << ch)) == 0u) continue;
r = gps_nav_epoch_assembly_push(&assembly, counter + ch,
local + ch, ch, ready,
period, window, holdoff);
}
if(r == GPS_NAV_EPOCH_EVENT_COMPLETE)
{
completed++;
}
else
{
discarded++;
}
uint32_t missing = assembly.expected_mask & ~assembly.received_mask;
for(unsigned int ch = 0u; ch < 4u; ch++)
{
if(assembly.received_mask & (1u << ch)) miss[ch] = 0u;
else if(missing & (1u << ch)) miss[ch]++;
}
counter += period;
local += period;
}
*completed_out = completed;
*discarded_out = discarded;
*silent_miss_out = miss[SILENT];
}
/*
* Regression for a defect this suite originally missed: exclusion must be
* reversible. An excluded channel is absent from ready_mask, so
* gps_nav_epoch_assembly_push() rejects its events as INVALID and it never
* enters received_mask. Clearing the miss count from the epoch result alone
* therefore pins it as excluded forever - on the board this collapsed the
* expected set to three channels and stopped position output permanently.
* The miss count must be cleared on the event itself.
*/
static void an_excluded_channel_is_re_admitted_when_it_emits_again(void)
{
const uint64_t period = 25000000ull;
const uint64_t window = 3000000ull;
const uint64_t holdoff = 4000000ull;
const unsigned int RECOVERED = 3u;
const unsigned int LIMIT = 2u;
unsigned int miss[8];
memset(miss, 0, sizeof(miss));
/* Start with the channel already excluded, as after a silent spell. */
miss[RECOVERED] = LIMIT;
uint64_t counter = 5000000000ull;
uint64_t local = 1000ull;
bool re_admitted = false;
for(unsigned int epoch = 0u; epoch < 6u; epoch++)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
uint32_t ready = 0u;
for(unsigned int ch = 0u; ch < 4u; ch++)
{
if(miss[ch] < LIMIT) ready |= (1u << ch);
}
if((ready & (1u << RECOVERED)) != 0u) re_admitted = true;
for(unsigned int ch = 0u; ch < 4u; ch++)
{
if((ready & (1u << ch)) == 0u)
{
/* The channel is emitting again even though it is excluded.
* This is the moment the miss count must be cleared - modelling
* what the firmware now does in the nav-solve event handler. */
if(ch == RECOVERED) miss[ch] = 0u;
continue;
}
(void)gps_nav_epoch_assembly_push(&assembly, counter + ch,
local + ch, ch, ready,
period, window, holdoff);
}
counter += period;
local += period;
}
CHECK(re_admitted,
"an excluded channel was never re-admitted after it resumed emitting");
CHECK(miss[RECOVERED] == 0u,
"a recovered channel kept a non-zero miss count");
}
/*
* Regression for the second board failure: excluding a silent channel must
* never take the expected set below GPS_NAV_MIN_WLS_SATELLITES. With four
* satellites, dropping one left three and made a solution impossible, where
* waiting for the channel to recover would have produced a fix.
*/
static uint32_t ready_mask_with_guard(const unsigned int* miss,
unsigned int channels,
unsigned int limit)
{
uint32_t eligible = 0u, active = 0u;
unsigned int active_count = 0u;
for(unsigned int ch = 0u; ch < channels; ch++)
{
eligible |= (1u << ch);
if(miss[ch] < limit)
{
active |= (1u << ch);
active_count++;
}
}
return (active_count >= GPS_NAV_MIN_WLS_SATELLITES) ? active : eligible;
}
static void exclusion_never_drops_below_the_wls_minimum(void)
{
const unsigned int LIMIT = 2u;
unsigned int miss[8];
/* Five channels, one silent: excluding it still leaves four. */
memset(miss, 0, sizeof(miss));
miss[2] = LIMIT;
uint32_t mask = ready_mask_with_guard(miss, 5u, LIMIT);
CHECK((mask & (1u << 2)) == 0u,
"a silent channel was kept when four others remained");
CHECK(mask == 0x1bu, "unexpected ready mask with five channels");
/* Four channels, one silent: excluding it would leave three, below the
* minimum, so it must be kept and waited for instead. */
memset(miss, 0, sizeof(miss));
miss[2] = LIMIT;
mask = ready_mask_with_guard(miss, 4u, LIMIT);
CHECK((mask & (1u << 2)) != 0u,
"exclusion dropped the expected set below the WLS minimum");
CHECK(mask == 0x0fu, "guard did not fall back to the eligible set");
/* Four channels, two silent: same reasoning, keep both. */
memset(miss, 0, sizeof(miss));
miss[1] = LIMIT;
miss[3] = LIMIT;
mask = ready_mask_with_guard(miss, 4u, LIMIT);
CHECK(mask == 0x0fu, "guard failed with two silent channels");
/* Six channels, two silent: four remain, so both are excluded. */
memset(miss, 0, sizeof(miss));
miss[1] = LIMIT;
miss[3] = LIMIT;
mask = ready_mask_with_guard(miss, 6u, LIMIT);
CHECK(mask == 0x35u, "guard excluded too few with six channels");
}
static void a_silent_channel_blocks_every_epoch_until_excluded(void)
{
const unsigned int EPOCHS = 12u;
const unsigned int LIMIT = 2u; /* GPS_NAV_SILENT_CHANNEL_EPOCHS */
unsigned int completed = 0u, discarded = 0u, silent_miss = 0u;
/* The defect: without exclusion, one silent channel costs every epoch. */
run_silent_channel_epochs(0u, EPOCHS, &completed, &discarded,
&silent_miss);
CHECK(discarded == EPOCHS,
"the silent-channel defect did not reproduce");
CHECK(completed == 0u,
"an epoch completed despite a permanently silent channel");
/* The fix: only the first LIMIT epochs are lost. */
run_silent_channel_epochs(LIMIT, EPOCHS, &completed, &discarded,
&silent_miss);
CHECK(silent_miss >= LIMIT, "the silent channel was never identified");
CHECK(discarded == LIMIT,
"a silent channel kept discarding epochs after exclusion");
CHECK(completed == (EPOCHS - LIMIT),
"epochs did not resume once the silent channel was excluded");
}
int main(void)
{
a_silent_channel_blocks_every_epoch_until_excluded();
an_excluded_channel_is_re_admitted_when_it_emits_again();
exclusion_never_drops_below_the_wls_minimum();
outage_matrix_rejoins_on_the_correct_grid();
guard_band_refuses_arming_near_a_bit_edge();
seeded_grid_equals_the_uninterrupted_grid();
a_wrong_seed_is_corrected_by_the_first_subframe();
a_stale_seed_cannot_survive_a_drop();
stale_context_is_refused_by_the_snap();
a_seeded_channel_joins_a_live_coalescing_epoch();
if(failures == 0)
{
puts("gps warm reacquisition simulation passed");
}
return failures == 0 ? 0 : 1;
}