diff --git a/hardware/external/gps/rtl/apb_wrappers/tracking_wrapper.sv b/hardware/external/gps/rtl/apb_wrappers/tracking_wrapper.sv index 49c5b31..d94365d 100644 --- a/hardware/external/gps/rtl/apb_wrappers/tracking_wrapper.sv +++ b/hardware/external/gps/rtl/apb_wrappers/tracking_wrapper.sv @@ -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 diff --git a/hardware/external/gps/rtl/apb_wrappers/trck_pkg.sv b/hardware/external/gps/rtl/apb_wrappers/trck_pkg.sv index 0e75700..7234978 100644 --- a/hardware/external/gps/rtl/apb_wrappers/trck_pkg.sv +++ b/hardware/external/gps/rtl/apb_wrappers/trck_pkg.sv @@ -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; diff --git a/hardware/external/gps/rtl/gps_timer/nav_solve_timer.sv b/hardware/external/gps/rtl/gps_timer/nav_solve_timer.sv index d1324fd..40a45a2 100644 --- a/hardware/external/gps/rtl/gps_timer/nav_solve_timer.sv +++ b/hardware/external/gps/rtl/gps_timer/nav_solve_timer.sv @@ -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 diff --git a/hardware/external/gps/rtl/gps_timer/tb/nav_solve_timer_seed_tb.sv b/hardware/external/gps/rtl/gps_timer/tb/nav_solve_timer_seed_tb.sv new file mode 100644 index 0000000..3782b93 --- /dev/null +++ b/hardware/external/gps/rtl/gps_timer/tb/nav_solve_timer_seed_tb.sv @@ -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 diff --git a/hardware/external/gps/rtl/readme.md b/hardware/external/gps/rtl/readme.md index 072cd8c..ef6ce53 100644 --- a/hardware/external/gps/rtl/readme.md +++ b/hardware/external/gps/rtl/readme.md @@ -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 diff --git a/hardware/external/gps/rtl/trck_src/tb/Makefile b/hardware/external/gps/rtl/trck_src/tb/Makefile index 205c54c..1050517 100644 --- a/hardware/external/gps/rtl/trck_src/tb/Makefile +++ b/hardware/external/gps/rtl/trck_src/tb/Makefile @@ -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) diff --git a/hardware/external/gps/rtl/trck_src/tb/tracking_wrapper_seed_tb.sv b/hardware/external/gps/rtl/trck_src/tb/tracking_wrapper_seed_tb.sv new file mode 100644 index 0000000..7b8c220 --- /dev/null +++ b/hardware/external/gps/rtl/trck_src/tb/tracking_wrapper_seed_tb.sv @@ -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 diff --git a/soc/software/gps_system/Makefile b/soc/software/gps_system/Makefile index f983741..d035765 100644 --- a/soc/software/gps_system/Makefile +++ b/soc/software/gps_system/Makefile @@ -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) diff --git a/soc/software/gps_system/README.md b/soc/software/gps_system/README.md index ee207cb..90f95b0 100644 --- a/soc/software/gps_system/README.md +++ b/soc/software/gps_system/README.md @@ -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. diff --git a/soc/software/gps_system/docs/RECEIVER_TIMING_AND_REACQUISITION.md b/soc/software/gps_system/docs/RECEIVER_TIMING_AND_REACQUISITION.md new file mode 100644 index 0000000..ed8d003 --- /dev/null +++ b/soc/software/gps_system/docs/RECEIVER_TIMING_AND_REACQUISITION.md @@ -0,0 +1,1333 @@ +# Receiver timing, ephemeris lifetime and warm/hot reacquisition + +Reconstructed from the RTL, HAL and firmware in this repository, not from +assumption. Every claim below names the file it was read from. Genesys2 is the +reference target; nothing here is board specific unless it says so. + +--- + +## 1. Architecture map + +| Concern | Where | +| --- | --- | +| Firmware entry | `gps_system.c`, `src/gps/gps_receiver.c` | +| Acquisition scheduling / PRN state | `src/gps/gps_acq_manager.c` | +| ACQ + TRCK register access, WLS driver | `src/gps/gps_ip.c` | +| ISR, event queue, epoch assembly | `src/gps/gps_interrupt_handlers.c` | +| Navigation epoch algebra | `src/gps/gps_nav_epoch.c` | +| HOW/sample time anchors | `src/gps/gps_time_anchor.c` | +| Ephemeris RAM cache | `src/gps/gps_ephemeris_cache.c` | +| Ephemeris ICD validity | `src/gps/gps_ephemeris_validity.c` | +| PPS discipline + capture read | `src/gps/gps_pps.c` | +| WLS / satellite position | `src/gps/NavEquations.c` | +| Tracking IP | `hardware/external/gps/rtl/trck_src/` | +| Tracking APB wrapper | `hardware/external/gps/rtl/apb_wrappers/tracking_wrapper.sv` | +| Nav-solve epoch timer | `hardware/external/gps/rtl/gps_timer/nav_solve_timer.sv` | +| Receiver sample counter | `hardware/external/gps/rtl/common/sample_counter.sv` | +| PPS block | `hardware/external/gps/rtl/pps/pps_wrapper.sv` | + +Board constants: 9 tracking channels, 1 acquisition engine, RF sample rate +16 367 981 Hz (`Navigation.h`), PPS counter clock 125 MHz (`gps_pps.h`), CLINT +timebase 500 kHz, nav-solve multiplier 10 (`GPS_NAV_SOLVE_TIMER_TICKS`, so a +200 ms navigation epoch), foreground systick 10 Hz. + +--- + +## 2. The receiver timebase + +Two independent counters carry receiver time. They are not interchangeable. + +**ABS_SAMPLE** — `common/sample_counter.sv` instantiates *one* 32-bit counter +for the whole receiver, clocked by the AFE sample strobe, shared by every +tracking channel. It is **not** reset when a channel starts, so it is a +continuous receiver-wide timeline. At 16.368 MHz it wraps every **262.4 s**; +that period is the hard ceiling on any unambiguous sample-domain extrapolation, +and it is why `GPS_HOW_CONTINUITY_MAX_GAP_SEC` is 120 s. + +Each channel latches ABS_SAMPLE at its nav-bit boundary (`trck_src/bit_gen.sv`) +and again into the APB register on a subframe-ready or nav-solve pulse +(`tracking_wrapper.sv:480`). REM_CODE_PHASE is latched on the same pulse, so +`gps_correct_abs_sample()` recovers the sub-sample code phase from a coherent +pair. + +**PPS counter** — a free-running 64-bit counter in the 125 MHz PPS domain, +captured per channel on the nav-solve pulse. This is the epoch identity used by +the coalescing logic. + +### 2.1 Is the counter read CDC-safe and atomic? Yes — verified from RTL + +This was the open Phase 3 question. The path is safe as built; no RTL change is +needed. + +1. The PPS counter is Gray-coded in its own domain + (`pps_wrapper.sv:100`), crossed through a two-stage `ASYNC_REG` chain + (`raw_gray_sync1_p`/`raw_gray_sync2_p`) and converted back to binary in + PCLK. Gray coding makes the multi-bit transfer coherent by construction: a + synchronised sample is always exactly N or N-1, never a mixture. +2. `capture_i[ch]` is **not** a clock crossing. It is + `nav_solve_timer_reached`, produced by `nav_solve_timer.sv` which already + runs in PCLK, and `pps_wrapper` consumes it in PCLK. +3. Counter and sequence are written in the same PCLK `always_ff` + (`pps_wrapper.sv:236`), so the capture register pair is atomic in hardware. +4. Firmware reads sequence / lo / hi / sequence and rejects the sample if the + sequence moved (`gps_pps_read_capture`, `gps_pps.c:277`). That is a correct + seqlock over three APB transactions. + +The ABS_SAMPLE crossing (`tracking_wrapper.sv`) is a plain 32-bit two-flop +synchroniser with no Gray code, which would normally be a defect. It is safe +here for a specific reason worth recording: ABS_SAMPLE is quasi-static — it +changes once per 20 ms nav bit and holds — and the PCLK capture happens at +`bit_vld_cc_p[0] & !bit_vld_cc_p[1]`, which is one PCLK *later* than the +synchroniser output settles. That extra cycle covers a one-cycle skew between +individual bits. The margin is real but it is implicit; anyone retiming this +wrapper must preserve it. + +**Contract:** a measurement is (PPS capture counter, capture sequence, raw +ABS_SAMPLE, REM_CODE_PHASE, PRN, status flags, CLINT timestamp), all latched by +one hardware pulse and read in one ISR pass (`trck_intr_handler`). + +--- + +## 3. Navigation epochs + +`nav_solve_timer.sv` does more than divide by N. On leaving IDLE it seeds its +counter with `(HOW Z-count * 300) mod N`, so every channel — whichever subframe +it happened to lock onto — produces nav-solve pulses on the **same absolute +navigation-bit grid**. Epochs are globally aligned to GPS time, not to when +each channel started. + +Firmware collects those pulses in `gps_nav_epoch_assembly_push` +(`gps_nav_epoch.c`), keyed on the PPS capture counter with an explicit +`deadline_local_ticks`. It already handles duplicate, stale, outlier, restarted +and complete outcomes, removes a channel that drops mid-epoch, and never waits +past its deadline. This part of the design is sound and was left alone. + +--- + +## 4. The 68 ms start offset + +`STARTOFFSET 68.802` (ms) is used in exactly two places (`gps_ip.c:956`, +`gps_ip.c:1253`). It is *not* a measured hardware delay and not a per-channel +property. It is a synthetic common-mode datum: pseudoranges are formed as +`(t_i - t_reference + STARTOFFSET)`, which makes every pseudorange land near a +plausible 68 ms transit time. Because it is identical for all channels it is +absorbed entirely by the WLS receiver-clock-bias state; the fitted bias is +`c*STARTOFFSET - reference_range`. It carries no timing information and cannot +go stale across reacquisition, because the real timing comes from the HOW/sample +anchor. + +### 4.1 Decision: retained, not replaced + +**The free-running counters do not replace `STARTOFFSET`, and were never meant +to.** The two solve different problems: + +* `STARTOFFSET` concerns absolute *receive* time, which this receiver never + determines. Receive time does not appear in the pseudorange construction at + all — `gps_ip.c:617` forms a **transmit-time difference** against a reference + satellite plus a constant plausible transit. Identical on every channel, so + WLS absorbs it entirely into `b`. +* The free-running counters (32-bit ABS_SAMPLE, 64-bit PPS) yield *differences*, + not an absolute datum. They anchor HOW→transmit time, identify epochs, and now + drive the warm-start seed and PRN context. None of that produces an absolute + receive epoch, so none of it can displace `STARTOFFSET`. + +It is also a per-solve constant, recomputed every epoch rather than carried +state, so it cannot go stale across a reacquisition — which is why the warm-start +work did not need to touch it. + +**Live confirmation (2026-08-08, Genesys2):** the model predicts a fitted bias of +`c × STARTOFFSET − reference_range` ≈ 20 626 km − ~20 372 km ≈ **254 km**. The +board reported `filtered_bias ≈ 253.686 km`. The documented model matches +measured data. + +Replacing it is **out of scope and not planned**. Should it ever be wanted, the +work is: + +1. Use the PPS-disciplined absolute receive time: `pseudorange_i = (t_rx − t_tx_i) × c`. +2. Solve the cold-start bootstrap: PPS discipline needs a fix, and the fix would + then need the PPS. +3. Retune for `b` collapsing from ~254 km to a genuinely small bias — + `gps_pps_time_filter` (whose header states this datum explicitly), the + residual gates, and the ESKF/GPS adapter covariances. + +That is a distinct change carrying real regression risk to a working fix. + +--- + +## 5. Reacquisition: what actually gates it + +Sequence when a tracked PRN drops and returns (`gps_interrupt_handlers.c`): + +1. Drop IRQ → node deleted, HOW anchor destroyed, channel freed. +2. Acquisition re-finds the PRN. +3. `assign_tracking_from_result` restores the cached ephemeris into the + channel's frame, so `ephemeris_valid` is true **immediately**. +4. First nav-bits IRQ (one subframe, ≤ 6 s) creates the node and the HOW + anchor. +5. Nav-solve interrupts are enabled and the channel joins the next epoch. + +So the receiver **already does not wait for subframes 1–3** — the RAM cache +covers that, and the effective wait is one subframe, not a full ephemeris set. +The measured floor is therefore ~6 s worst case, ~3 s typical, versus 18–30 s +if the cache is missed. + +**Getting below that is blocked in hardware, not firmware.** +`nav_solve_timer.sv:72` will only leave IDLE on `subframe_ready && trck_busy`, +and it drops back to IDLE whenever `trck_busy` deasserts — which is exactly what +a reacquisition does. Until the channel decodes one valid subframe, the IP emits +no nav-solve pulse, so there is no measurement to place in an epoch, no PPS +capture, and nothing for firmware to accept. Firmware and hardware are currently +in lockstep at the same one-subframe boundary; removing the firmware-side wait +alone would buy nothing. + +### 5.1 RTL change — approved and implemented + +1. **Problem.** After a channel restart, `nav_solve_timer` cannot resume until + a fresh `subframe_ready`, costing up to 6 s even when the receiver already + knows GPS time, has valid ephemeris for that PRN, and the channel is locked + and bit-synchronised. +2. **Why firmware/HAL cannot fix it.** The gate is internal to the IP. The + epoch phase is derived from `subframe_count` (the HOW Z-count) inside the + timer; software has no register that seeds the counter or forces the state + machine into COUNT. There is no alternative pulse to timestamp. +3. **Smallest change.** Add one write-only APB register to `tracking_wrapper`, + `NAV_SOLVE_PHASE_SEED` (32-bit value + one arm bit). When armed, + `nav_solve_timer` may also leave IDLE on the armed seed, loading + `internal_counter_p` with the supplied phase instead of computing it from + `subframe_count`. The existing `subframe_ready` path stays untouched and + still overrides the seed on the next real subframe, so the hardware remains + self-correcting. Firmware computes the seed from the persisted PRN anchor + and the current receiver time. +4. **Affected modules.** `apb_wrappers/tracking_wrapper.sv` (one register, one + decode case), `gps_timer/nav_solve_timer.sv` (one extra IDLE exit and one + mux on the counter load). No datapath, no new clock, no change to the + measurement or PPS capture path. +5. **CDC/timing.** The register is written and consumed in PCLK; the timer + already runs in PCLK. The seed is quasi-static and only sampled on the IDLE + exit. No new crossing, no new asynchronous path, no effect on the tracking + domain. The arm bit must be cleared by hardware on the IDLE exit so a stale + seed cannot re-arm. + +The correctness precondition is that the reacquired channel's bit grid is the +satellite's true grid — which `bit_gen.sv` guarantees, since it syncs on a real +1→0 navigation-bit transition. Without that, seeding would place measurements on +a grid offset by a multiple of 20 ms. + +### 5.2 Seed contract + +`phase_seed_value` is **the counter value in effect for the next navigation bit +after the timer consumes the seed**. The hardware raises an epoch on the bit +whose counter value is `nav_solve_ticks - 1`, so + +``` +seed = (absolute navigation-bit index of the next bit) mod nav_solve_ticks +``` + +This is identical to the subframe path, where the ALIGN divider produces +`(Z_count * 300) mod N` and `Z_count * 300` is exactly the absolute index of the +first bit after the subframe boundary. Both paths therefore land on absolute +navigation-bit indices `b` with `(b + 1) mod N == 0` — one global grid. + +Hardware guarantees, each covered by a Questa test: + +* the seed is answered in one cycle, in **every** state, so an armed request has + a lifetime of one evaluation and cannot be applied to a later reacquisition; +* it is only *applied* from IDLE, and a decoded subframe in the same cycle wins; +* an unreduced seed (`>= counter_max_val`) is refused rather than aliased, and + still clears the arm bit so firmware cannot wedge waiting for an answer; +* a seeded phase is marked provisional, and the first decoded subframe re-runs + the authoritative HOW alignment and clears the flag — a wrong seed + self-corrects within one subframe instead of persisting for the whole lock; +* `trck_busy` deasserting returns the timer to IDLE and clears provisional. + +### 5.3 Firmware side + +State is split by lifetime, as the design called for: + +* **PRN context** (`gps_prn_context[]`, survives channel reassignment) — the + last confirmed nav-solve epoch for that satellite: transmit time, PPS capture + counter and RF sample latch, all captured by one hardware pulse and therefore + internally coherent. +* **Channel warm-start state** (`trk_seed_requested`, `trk_seed_unconfirmed`, + `trk_seed_prn`) — cleared on drop, never allowed to outlive the channel. + +Flow on reacquisition: cached ephemeris is restored and qualified (section 6); +if a PRN context exists, a seed request is raised. The foreground arms it once +the channel reports busy+lock. The phase is computed **entirely in the PPS +counter domain** — that counter is 64-bit and never wraps, 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. + +Three independent guards stand between a bad seed and a bad fix: + +1. **Guard band.** The arm is refused within 4 ms of a navigation-bit boundary, + so the applied phase cannot be one bit late. A refused attempt simply retries + on the next foreground pass. +2. **Grid snap.** The first seeded epoch's predicted transmit time is snapped to + the known nav-solve grid and the residual must be under 50 ms. The hardware + can only raise that epoch on a multiple of the nav-solve period, so a + prediction that does not land there proves the context is stale. On failure + the channel reverts to waiting for a subframe. +3. **Exclusion until proven.** A seeded channel is kept out of + `gps_nav_ready_channel_mask()` until its first epoch validates, so an + unproven seed can never hold up a coalescing epoch. + +Once the first epoch validates, its transmit time and ABS_SAMPLE become an +ordinary HOW/sample anchor and every later epoch uses the existing, already +tested sample-anchor path — including its 5 ms snap check. The anchor is +committed with `gps_how_week_valid` clear, because the week still comes from the +PRN context rather than a parity-checked subframe 1; the first decoded subframe +upgrades it and the existing HOW-continuity check confirms or rejects it. + +Diagnostics: `warm_seed_armed_total`, `warm_seed_confirmed_total`, +`warm_seed_rejected_total`. + +### 5.4 Resulting latency + +A returning PRN with valid cached ephemeris and a live PRN context joins the +next nav-solve epoch — **200 ms at the configured multiplier of 10** — instead of +waiting up to 6 s for a subframe. The true lower bound is one nav-solve period +plus the time for the channel to report lock, because the seed cannot be armed +before the tracking loop has locked and the epoch grid is 200 ms coarse. + +The fast path is refused, and the receiver falls back to the subframe wait, when +there is no cached ephemeris, no PRN context, the context is older than 15 +minutes, the ephemeris is outside its fit interval, or any of the three guards +above rejects. + +--- + +## 6. Ephemeris lifetime (implemented in this change) + +Cache key was already PRN, not channel (`gps_ephemeris_cache.h`), so channel +reassignment never destroyed usable ephemeris. What was missing was any +GPS-time notion of validity: the only test was "decoded less than two hours of +systick ago", which measures decode age, not fit validity, and would happily +survive a receiver-time jump. + +Added `gps_ephemeris_validity.{h,c}`: + +* `gps_ephemeris_fit_interval_hours()` — IS-GPS-200 Table 20-XI/20-XII. Fit + flag clear is the nominal 4 h curve fit. Fit flag set uses the tabulated + IODC ranges (240–247 → 8 h, 248–255 and 496 → 14 h, 497–503 → 26 h, + 504–510 → 50 h, 511 → 74 h). **Project assumption, documented in the header:** + a set fit flag with an IODC outside those ranges falls back to 6 h, which is + shorter than the ICD's smallest tabulated long fit and therefore conservative. +* `gps_ephemeris_toe_offset_sec()` — signed half-week difference, so Toe near a + week boundary and receiver time just past it gives the true small age instead + of nearly a week. LNAV does not broadcast a week for Toe, and this avoids + needing one. +* `gps_ephemeris_check_static()` — committed flag, PRN identity, IODE equal to + the low eight bits of IODC, health, URA index < 15, orbital parameter sanity. + Runs without receiver time, so a flash record can be qualified at boot. +* `gps_ephemeris_check()` — the above plus `|t - Toe| <= fit/2`. +* `gps_ephemeris_supersedes()` — a re-decode of the same data set is *not* new + information; only a changed issue of data with an advancing Toe (or a health + or URA change inside one issue) replaces the cached record. This is the write + trigger the flash wear policy will need. + +Wiring: + +* `gps_ephemeris_cache_store_if_new()` replaces the unconditional store in the + nav-bits path and reports whether content actually changed. +* `gps_ephemeris_cache_restore_at()` qualifies a restored record against + current receiver time on channel assignment; an age-rejected record is + dropped rather than reused. +* `gps_ephemeris_ready_for_position_at()` applies the fit window at the epoch's + own transmit time in `gps_collect_completed_epoch_locked()`, so an ephemeris + that expires between measurement creation and solve time is excluded. +* A coarse receiver GPS time datum is maintained from accepted nav-solve epochs + and extrapolated with the systick between them. It is only ever used to place + an ephemeris inside or outside a multi-hour window; the navigation solution + still uses per-channel transmit times. +* `subframe 2` word 10 bit 17 is now decoded into `fit_flag`. +* Diagnostics gained `ephemeris_cache_miss_total`, `ephemeris_rejected_total` + and the last rejecting PRN/reason, surfaced in the nav status line. + +Normal operation is unaffected: a broadcast ephemeris is transmitted with Toe +at the centre of its fit interval, so `|t - Toe|` stays well inside 2 h while +it is being received. + +--- + +## 6.1 Flash persistence + +`gps_ephemeris_store.{h,c}`, on the top two 64 KiB sectors of the 32 MiB +S25FL256S (`0x01FE0000`), clear of the bitstream at 0 and the application at +`0x01800000`. + +**What is stored is the three raw LNAV subframes, not the parsed structure.** +That keeps the 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 +decoder that produced it. To make that possible the LNAV decoder was split out +of the MMIO-heavy `gps_ip.c` into `gps_lnav_parser.c`, so it links into host +tests and into the store without dragging in the tracking driver. + +Format: two erase sectors as an append-only log pair. Each opens with a header +slot carrying a generation number; the higher valid generation is live. Records +occupy one 256-byte page and are committed by their CRC-32, 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 at generation+1 and only then is +the old sector erased — a power loss at any point leaves a mountable store. + +Wear: an erase happens only on first format and on compaction. A record reaches +flash only when `gps_ephemeris_supersedes()` says the data set is genuinely new, +so re-decoding the same broadcast costs nothing. With ~8 tracked satellites +uploading every ~2 h, a 256-slot sector fills in roughly 2–3 days, so the two +sectors see on the order of one erase every few days against a ~100k-cycle +endurance. + +Boot: records load into the RAM cache as **candidates**. They are not trusted +for navigation until receiver time exists — +`gps_ephemeris_cache_restore_at()` then applies the curve fit window (section +6), and an out-of-fit record is dropped rather than used. + +**Caveat that matters on the board:** a 64 KiB sector erase on this device runs +for hundreds of milliseconds. Writes are therefore deferred out of the event +path and drained one record per background pass, but a compaction erase will +still stall the foreground. The ISR keeps queueing (64-deep queue, ~23 events +worst case during an erase) so nothing is lost, but expect two or three degraded +nav epochs when a compaction lands. Build with `GPS_EPHEMERIS_FLASH_ENABLE=0` +for firmware that never writes flash. + +Verified on a simulated device that enforces real NOR semantics — programming +only clears bits, and writes can be truncated mid-byte-stream: format, remount, +supersede, compaction, torn record, power loss at every program of the +compaction sequence, corrupt header, duplicate save, unhealthy/wrong-PRN +rejection, and a CRC-32 known-answer plus exhaustive single-bit-flip check. + +## 7. Clock bias versus code ambiguity + +WLS solves `[x, y, z, b]` with a common-mode clock bias, so a receiver clock +offset that is genuinely common to all channels is absorbed regardless of size. +That is *not* the same as tolerating an arbitrary outage, and the distinction +matters: + +* The anchor extrapolation converts an ABS_SAMPLE difference to elapsed time + and then snaps to the nav-solve grid with a 5 ms tolerance + (`gps_time_from_periodic_sample_anchor`). Satellite range rate contributes + `≈2.7e-6 * Δt`; over 262 s that is 0.7 ms — inside the tolerance. Sample + clock error at a few ppm over the same interval is of the same order. So the + snap is the binding constraint well before the clock bias is. +* The ABS_SAMPLE counter wraps at 262.4 s. Beyond one wrap the elapsed time is + ambiguous by 262.4 s, which is not recoverable from the sample domain alone. + Any reuse of a persisted anchor must therefore be bounded below one wrap, or + disambiguated by the PPS counter (64-bit, no practical wrap). +* The 1 ms C/A period bounds anything derived from code phase alone: a receive + time uncertain by more than ±0.5 ms cannot be assigned to the correct code + period without an independent integer resolution step. + +This is why "WLS will fix it" is not a sufficient answer for the 1 min / 5 min +outage cases: the clock bias is fine, the epoch identity is not. + +--- + +## 8. Build and test + +```sh +# Host models and unit tests +make -C soc/software/gps_system/tests run +make -C soc/software/gps_system/tests sanitize + +# RTL simulation (Questa). Covers the epoch timer, the seed state machine and +# the APB register contract, plus the pre-existing measurement testbenches. +make -C hardware/external/gps/rtl/trck_src/tb test +make -C hardware/external/gps/rtl/pps/tb test + +# Bare-metal firmware +make -C soc/software/gps_system + +# Genesys2 image +make FPGA_BOARD=genesys2 APP=gps_system fpga-mcs +``` + +RTL testbenches: + +| Testbench | Covers | +| --- | --- | +| `gps_timer/tb/nav_solve_timer_seed_tb.sv` | 9 cases: subframe path unchanged, seed starts epochs, seeded grid equals the subframe grid in absolute bit indices, unreduced seed refused, busy/enable required, late seed ignored, subframe overrides a wrong seed, seed cannot survive a drop, subframe wins a same-cycle tie | +| `trck_src/tb/tracking_wrapper_seed_tb.sv` | APB register read/write, arm bit self-clears, provisional status bit, seeded epochs raise real interrupts and PPS captures, stale seed does not restart after a drop, out-of-range seed refused | + +Host models: + +| Test | Covers | +| --- | --- | +| `test_gps_warm_reacquisition_sim.c` | Behavioural model of `nav_solve_timer.sv` driven by the real firmware functions, so RTL and firmware are cross-checked rather than each verified alone: outage matrix 20 ms → 900 s at 0/±2 ppm, seeded grid identical to an uninterrupted channel, guard band, wrong-seed correction by subframe, stale seed after a drop, coalescing with a joining channel, drops mid-epoch, stale and duplicate events | +| `test_gps_ephemeris_store.c` | Simulated NOR device with bit-clearing semantics and power-loss truncation | +| `test_gps_time_anchor.c` | Seed phase, guard band, epoch snap, drift bounds | +| `test_gps_ephemeris_validity.c` | Fit interval table, week rollover, supersede | + +--- + +## 8.1 Board validation, 2026-08-08 (Genesys2, live sky) + +Bitstream from the 00:11 build (postdates the RTL change), programmed to FPGA +SRAM; firmware loaded over JTAG into DTIM; live antenna. + +**The RTL register is real.** `NAV_SOLVE_PHASE_SEED` at `0x10021074` reads 0, +accepts `0x5A`, reads it back, restores. `trck0 STATUS = 0xdb` decodes to +BUSY+LOCK+NAV_BITS_RCV+NAV_SOLVE_FLAG+MEASUREMENT_FLAG with the new +`NAV_SOLVE_PROVISIONAL` bit clear — correct for a subframe-aligned channel. + +**Cold start unaffected.** Full fix, PPS locked, 8 satellites, GDOP 2.38, +disciplined at -1.56 ppm. `ephemeris_rejected_total = 0` throughout, so the new +IS-GPS-200 fit-interval gate excludes nothing it should not. + +**Warm seed, forced drop of PRN 10** (stop bit written by read-modify-write so +the channel's enables were preserved): + +| Counter | before | after | +| --- | ---: | ---: | +| `satellite_lost_total` | 1 | 2 | +| `last_lost_prn` | 12 | 10 | +| `ephemeris_cache_restore_total` | 0 | 1 | +| `warm_seed_armed_total` | 0 | 1 | +| `warm_seed_confirmed_total` | 0 | 1 | +| `warm_seed_rejected_total` | 0 | 0 | + +The seed register held the computed phase (1), the first seeded epoch passed the +grid snap, and the channel came back with `NAV_SOLVE_PROVISIONAL` already +cleared — a real subframe had re-aligned it, which is the designed +self-correction. Navigation improved across the event (GDOP 5.36 -> 2.38). + +**Flash persistence.** Controlled reboot test: six records written, reset, then +`flash_ephemeris_loaded_total = 6`, store `scanned = 6 / rejected = 0`, +`ephemeris_cache_restore_total = 3` against only 1 miss (cold boots show 6-9 +misses). Steady state after a minute: 7 writes, 5 restores, 2 misses, 0 errors, +0 erases, 0 compactions. Satellites are navigation-eligible on acquisition +without re-decoding subframes 1-3. + +**Diagnostics are only readable at `GPS_DEBUG_LEVEL=3`, which is unusable on +this board** — level 3 saturates the 460800 baud UART, starves the foreground +and drives every channel stale (`tracked=0 stale=5`). All the warm-seed and +flash results above were read from `gps_runtime_diagnostics` over JTAG at level +1 instead. That is what the structured counters are for; do not chase these +paths with printf. + +**Unexplained, carried forward:** on the first flash-enabled boot the store +mounted with `generation = 1` and `erases = 0`, i.e. it found an already +formatted store with one record that no flash-enabled run in that session should +have written. Blank flash cannot be mistaken for a valid store (magic would read +`0xffffffff`), and GPS init demonstrably runs once, so this is not yet accounted +for. It does not affect the controlled reboot result above, but the next clean +run should erase the two sectors first and confirm `erases = 2` at format. + +## 8.2 Follow-up work, 2026-08-08 + +Six items were triaged after the first board session and all were implemented. + +**Event queue.** `GPS_EVENT_QUEUE_SIZE` 64 -> 128. The 64-entry queue overflowed +6-7 times during the startup burst, and the margin mattered more once flash +compaction became possible: a sector erase stalls the foreground for hundreds of +ms, during which nine channels contribute ~23 nav-solve events alone. Overflow +is safe by itself - tracking events fall back to per-channel shadow buffers - +but a lost `nav_bits` event costs a subframe. **Board result: +`event_overflow_total = 0`, previously 6-7.** + +**Split seed-rejection counters.** `warm_seed_rejected_total` now breaks down +into `warm_seed_refused_total` (the tracking IP declined the armed seed, i.e. +the channel lost lock between the busy/lock check and the arm) and +`warm_seed_snap_failed_total` (the first seeded epoch missed the nav-solve grid, +i.e. the PRN context no longer describes that satellite). The two have entirely +different meanings and were previously indistinguishable. + +**Early issue-of-data cross-check.** `gps_lnav_broadcast_iode()` extracts the +IODE from a raw subframe without staging or committing it - subframe 1 via the +low byte of IODC, subframes 2 and 3 directly. The nav-bits path compares it +against the ephemeris in use before `parse_subframe()` can replace anything, and +counts `ephemeris_iod_mismatch_total`. This detects within one subframe, rather +than a full set, that a record (typically flash restored) has been superseded. + +Navigation behaviour deliberately does not change: IS-GPS-200 permits using an +older ephemeris inside its curve fit interval, and that window is already +enforced per epoch. The value is observability and a faster refresh decision. + +**Flash test hooks.** Two build options, both required to be 0 in production: + +* `GPS_EPHEMERIS_FLASH_FORCE_FORMAT=1` erases both sectors at boot. +* `GPS_EPHEMERIS_STORE_MAX_SLOTS=N` caps usable slots per sector, so compaction + is reachable in seconds instead of after 256 records - while still performing + real 64 KiB erases through the same code path. + +**Board result - format path.** With `FORCE_FORMAT=1`: `erases = 2`, +`generation = 1`, `next_slot = 1`, `scanned = 0`. Two real sector erases via the +4-byte-address `0xDC` opcode, header written, store empty. This also settles the +earlier unexplained `generation = 1`: mount from a genuinely blank store is +correct, so that observation was leftover state from an earlier run, not a mount +defect. + +**Plumbing defect found twice.** Both times a new build flag was added it was +silently dropped because `targets/fpga/genesys2/Makefile` did not forward it, so +the board build used the header default and the test appeared to do nothing. +`GPS_WARM_SEED_ENABLE`, `GPS_EPHEMERIS_FLASH_ENABLE`, +`GPS_EPHEMERIS_STORE_MAX_SLOTS` and `GPS_EPHEMERIS_FLASH_FORCE_FORMAT` are all +forwarded now. Anything added later must be added in three places: the header +`#ifndef` default, `soc/software/gps_system/Makefile`, and the target Makefile. + +## 8.3 Compaction on hardware, and a destructive bug it exposed + +With `GPS_EPHEMERIS_STORE_MAX_SLOTS=4` and `FORCE_FORMAT=1`, compaction ran twice +on the real device: `generation = 3`, `erases = 6`, `compactions = 2` - exactly +2 for the format plus 2 per compaction, with the generation advancing once each +time. The compaction path, including real 64 KiB erases, works. + +The same run also showed `flash_ephemeris_error_total = 2`, which turned out to +be a genuine defect rather than a test artefact: + +**Compaction could erase flash indefinitely.** When the live record set fills +the sector exactly, compaction *succeeds* but leaves no free slot. The append +that follows then fails, the save returns an error, and the next save compacts +again - erasing two more sectors. Nothing bounded that loop. On a device rated +around 100k erase cycles, a receiver in that state would consume them +continuously. + +Two things were wrong in the original code. The capacity check only covered the +case where the live set does not fit at all, and by then the spare sector has +already been erased; it did not cover the exact-fit case, which is the one that +actually occurred. Both now latch `capacity_exceeded`, which refuses all further +saves without touching the device, and the firmware disables persistence and +clears the pending mask when it sees the latch. A remount clears it, so recovery +is possible once the configuration or the live set changes. + +**Production settings cannot reach this** - 32 PRNs against 255 usable slots - +so it was only reachable because the slot cap made it reachable. That is +precisely what the test hook was for. A regression test drives the store into +the condition and asserts the erase count stops changing afterwards. + +**Event queue and erase stalls.** With the queue at 128, a run without flash +compaction showed `event_overflow_total = 0`, against 6-7 at 64: the startup +burst is now covered. A run *with* two compactions showed 6 overflows again, +which is the erase stall - an S25FL256S sector erase is specified in hundreds of +ms and can reach seconds, and nine channels generate roughly 45 nav-solve events +per second. Overflow is not data loss (per-channel shadow buffers hold the +newest drop, nav-bits and nav-solve event each), but a second nav-bits event +arriving inside one stall is dropped and costs a subframe. Sizing the queue for +a worst-case erase is not practical; the honest characterisation is that a +compaction costs a few degraded nav epochs, and compaction is rare - once every +few days at production slot counts. + +## 8.4 Clean measurement run (no JTAG halts) + +Production flags, roughly three minutes of untouched running, then **one** read. +Only the first halt after a load yields usable numbers - see the measurement +caveat below: + +| Counter | With JTAG halts | Clean run | +| --- | ---: | ---: | +| `nav_epoch_incomplete_total` | 140 | **1** | +| `measurement_missed_total` | 356 | **0** | +| `flash_ephemeris_error_total` | 2 | **0** | +| `satellite_lost_total` | 6 | 0 | + +So both were entirely artefacts of debugger halts, not firmware defects. The +receiver stayed healthy throughout: 6 satellites, PPS in `precision` mode, GDOP +2.83, locked. + +**Measurement caveat, quantified.** A later attempt to re-read the same counters +at 338 s uptime showed how large the effect is. Two reads seconds apart in one +session returned `measurement_missed` of 122 and then 5914: the second halt had +inherited the stall of the first. One `gdb -batch` connect/halt/read/resume +cycle therefore costs on the order of 5900 missed measurements, about 13 s of +frozen foreground at nine channels and 50 Hz. + +Consequences for anyone repeating this: + +* Only the **first** read after a load is clean. Every subsequent halt pollutes + `measurement_missed` and `nav_epoch_snapshot_overwrite`. +* `event_overflow_total` (4) and `nav_epoch_incomplete_total` (1) were identical + in the clean read and the contaminated one, so those two are robust against + halting and can be trusted from a later read. +* Continuous monitoring needs UART telemetry or a non-halting transport, not + JTAG. + +Two things remain non-zero and are honest observations rather than resolved +items: + +* `event_overflow_total = 4`. At queue 64 this was consistently 6-7; at 128 we + have observed both 0 and 4. Reduced, not eliminated - the startup burst can + still exceed 128 when many satellites appear at once. Safe (shadow buffers), + but not fixed. +* `nav_epoch_snapshot_overwrite_total = 40` in the clean read. This one is + itself inflated by the single halt that read it, so the true steady-state rate + is unknown and the figure should not be quoted as a defect rate. Measuring it + properly needs UART telemetry. + +## 8.5 Slow recovery after a total outage: two software root causes + +Symptom reported from the bench: after pulling the RF cable the receiver took +about a minute to print a position again, and position output also stalled for +seconds whenever a new satellite joined. Forced total-drop tests reproduced it, +with recovery stalls of 14 s, 36 s and, in the worst case, no recovery at all +for over two minutes. + +Finding these took four attempts. Three hypotheses were wrong - silent +channels holding the expected set, the 25 ms epoch collection window, and simply +a weak sky - and the reason they survived as long as they did is worth +recording. + +### The counter that caused the misdiagnosis + +`nav_epoch_incomplete_total` was incremented from **three** places, two of which +mean "the epoch assembly failed" and one of which means something completely +different: `gps_nav_publish_active_snapshot()` also incremented it when an epoch +assembled perfectly but carried fewer than `GPS_NAV_MIN_WLS_SATELLITES`. + +That third case dominates by an order of magnitude. On the board, 633 total +resolved into 563 "below minimum" against 70 real assembly failures - 89% of the +counter was a satellite-availability statement, not an epoch fault. Every +diagnosis built on it pointed at the epoch machinery, which was healthy all +along. + +It is now split into `nav_epoch_assembly_failed_total` and +`nav_epoch_below_minimum_total`; the original is kept for continuity but should +not be used. A single ambiguous counter cost three wrong diagnoses; that is the +lesson worth keeping from this episode. + +### Root cause 1: inverted acquisition retry priority + +`gps_acq_manager_retry_due()` served a HOT PRN - one tracked seconds ago and +therefore almost certainly still above the horizon - with +`hot_retry_delay_ticks` (30 s) from its **first** retry, while a COLD PRN that +had never been seen was retried every `retry_delay_ticks` (0.5 s). + +The best candidate available was penalised sixty times harder than the worst. +After a mass loss the known-good satellites went into a 30 s lockout while the +single acquisition engine spent ~0.5 s per attempt sweeping 26 cold PRNs that +mostly were not visible. The board log shows it exactly: `hot=2`, engine +cycling cold PRNs, `tracked` pinned. + +Fixed by making recency decay with **failures** rather than being discarded on +the first one: `GPS_ACQ_HOT_FAST_ATTEMPTS` (6) retries at the fast cadence +before the long backoff applies, reset when the PRN tracks again. The +regression test in `test_gps_acq_manager.c` was verified to fail against the old +policy and pass with the new one. + +### Root cause 2: circular dependency in nav-solve enablement + +`gps_ip.c` enabled nav-solve interrupts only inside the `else` branch of + +```c +if(ready_count < GPS_NAV_MIN_WLS_SATELLITES) { ... } +else { gps_enable_nav_solve_for_snapshot_locked(...); } +``` + +A channel cannot contribute to a navigation epoch until its nav-solve interrupt +is enabled, and the enable only ran once four channels were already +contributing. **Below four satellites the receiver could not climb back out.** + +This is why recovery was intermittent rather than consistently broken: the +warm-seed path enables nav-solve independently, so whenever a seed was confirmed +it masked the deadlock. That intermittency is what made the fault hard to see. + +Fixed by hoisting the enable out of the branch. Each channel is already +qualified individually by `gps_node_ready_for_position()` - 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`. + +**Verified on hardware:** with four channels tracking and no fix, all four read +`navsolve_irq_en=1`. Under the old code that state was unreachable. + +### Hardware was not the constraint + +Capture spread peaked at 12.7 ms against a 25 ms sync window, and epoch restarts +were zero. Neither fault needed an RTL change. The single acquisition engine +only sets the cost of a wasted attempt (~0.5 s/PRN); it was being pointed at the +wrong satellites and its results were being withheld from the solver. Adding +engines would have reduced the cost of the waste without removing it. + +### Not yet validated + +The **mechanism** of both fixes is verified; the **outcome** is not. Sky +conditions degraded through the session - 7-8 satellites early, 4 before the +last drop, 2 after - and below four satellites no fix is possible under any +code, so those runs cannot distinguish fixed from unfixed. A drop test with 7-8 +satellites is still owed, watching `nav_epoch_below_minimum_total` against +`nav_epoch_assembly_failed_total`. + +Root cause 2 carries the larger regression surface, because it changes when +nav-solve turns on for every channel including at cold start. It passes the +host suite and cold-started correctly on the board, but deserves a +good-constellation run before it is trusted. + +## 8.6 Position scatter is geometry, not a measurement regression + +Reported symptom: position scatter had grown. It had - but not because +anything in the measurement path got worse. + +Dividing horizontal scatter by HDOP gives an effective UERE, the per-satellite +ranging error. Across every capture on 2026-08-08, before and after the +section 8.5 fixes: + +| capture | satellites used | HDOP | drms | UERE | +|---|---|---|---|---| +| 12:08 | 8.4 | 1.03 | 9.2 m | 8.9 m | +| 12:24 | 6.7 | 1.66 | 13.5 m | 8.1 m | +| 12:42 | 5.1 | 4.46 | 32.6 m | 7.3 m | +| 13:29 | 7.0 | 2.58 | 24.0 m | 9.3 m | +| 13:40 | 6.2 | 1.62 | 11.6 m | 7.2 m | +| 13:50 | 6.5 | 1.47 | 11.0 m | 7.5 m | + +UERE is flat at 7-9 m throughout while scatter varies by 3.5x. The cleanest +demonstration is the 13:29 -> 13:40 pair: the **same binary**, simply +restarted, HDOP 2.58 -> 1.62 and scatter 24.0 m -> 11.6 m. + +A PCA of the worst run put 89% of the position variance on a single axis. That +is the signature of the weakly observed direction under poor geometry, not of +one bad channel - and it is consistent with the flat UERE. + +**Rule for future triage: scatter tracks HDOP. Treat a scatter increase as a +geometry story until UERE moves.** Chasing it as a measurement regression cost +real time here. + +## 8.7 Satellite count was not sky-limited: almanac-directed acquisition + +Cross-checking the tracked set against satellite visibility computed from a +live USCG YUMA almanac, for the receiver's own position and time: + +* 13 GPS satellites above the horizon, 12 above 5 degrees. +* The receiver tracked 8. All 8 were in the computed visible set, which also + confirmed that the logs print **0-based PRN indices** - the real PRN is + `index + 1`. +* Missed while visible: PRN 1 (34 deg elevation, azimuth 172), 19 (13 deg), + 2 (12 deg, azimuth 169), 17 (11 deg), 16 (4 deg). + +PRN 1 sat at 34 degrees and was searched 15 times in four minutes without ever +being acquired, while PRN 28 at 9 degrees and PRN 7 at 10 degrees were +acquired. Sensitivity alone does not fail a high satellite and succeed on low +ones. PRN 1 and PRN 2 lie at azimuth 169-172 while PRN 7 at azimuth 199 tracks +normally, which points at a local obstruction in a narrow southern sector - +antenna siting rather than constellation availability. + +### The firmware defect: 75% of acquisition effort was wasted + +Of 385 failed searches in 240 s, **287 (75%) were against PRNs below the +horizon**. With `ACQ_ENGINE_COUNT = 1` a full 32-PRN sweep takes about 16 s; +restricted to the visible set it takes about 4 s. + +The cause was that `parse_subframe()` read subframe 4/5 page headers and kept +only the page-18 ionosphere data and the page-25 almanac *reference* - every +almanac **orbital element was discarded**. The receiver was demodulating the +data that says which satellites are up, then throwing it away. + +### What was implemented + +* `src/gps/gps_almanac.c` decodes almanac pages (subframe 5 SV IDs 1-24, + subframe 4 SV IDs 25-32) and propagates them with the IS-GPS-200 user + algorithm to an elevation and azimuth. +* `gps_lnav_parser.c` stores every almanac page into a receiver-global + almanac, and lifts the 8-bit WNa into the full week using the frame's + ephemeris week. +* `gps_interrupt_handlers.c` recomputes a visible-PRN mask every 10 s from the + almanac, the resolved receiver time and the last WLS position, and publishes + it to the scheduler. +* `gps_acq_manager.c` retries a PRN predicted below the mask on a 20 s cadence + instead of the 0.5 s sweep. + +The prediction is **advisory by construction**, which is the whole design +constraint: + +* No almanac, no resolved time, or no position yet -> mask is all ones and + acquisition behaves exactly as before. +* A PRN with no almanac entry yet stays searchable, so a partially collected + almanac never hides a satellite. +* An all-zero predicted-visible mask is treated as "no usable prediction" + rather than as silence. +* An invisible PRN is **slowed, never excluded**, including one that has never + been attempted - so a stale almanac, a stale position or a decode error + costs search efficiency and self-corrects, but can never make a genuinely + visible satellite unreachable. + +### Verification + +`tests/test_gps_almanac.c` checks field decoding, page rejection, visibility +fallbacks, and propagation against an independent Python implementation of the +same user algorithm using a real broadcast almanac for PRN 1. + +That cross-check earned its keep immediately: the reference and the decoder +disagreed by 1417 km because the Python version omitted the IS-GPS-200 +20.3.3.5.2.3 requirement to wrap `tk` to the half week. The C was correct. + +`tests/test_gps_acq_manager.c` adds three cases covering the slow-but-never- +excluded property, the empty-mask fallback, and that a visible PRN keeps the +fast cadence. + +## 8.8 Persistent navigation context: almanac, ionosphere, last position + +Almanac-directed acquisition (section 8.7) only helps once an almanac exists, +and collecting one from the air takes 12.5 minutes because the 25 pages come +round one per 30 s frame. Without persistence every reset therefore swept the +whole constellation blind for that entire window - which is exactly when fast +reacquisition matters most. The same 12.5 minute cycle carries subframe 4 +page 18, so the ionosphere model was equally unavailable, leaving every +pseudorange with an uncorrected ionospheric delay after a restart. + +`src/gps/gps_nav_context_store.c` persists three things that share a lifetime +of weeks rather than hours: + +* the raw LNAV almanac page per PRN, +* the raw subframe 4 page 18 (ionosphere/UTC), +* the last converged WLS position, with the receiver time it was taken at. + +### Why not the ephemeris store + +The ephemeris store is an append log keyed per PRN with compaction. That shape +buys nothing for a small blob that is rewritten in full and rarely, and it +carries the complexity that produced a destructive erase-loop bug once already +(section 8.3). The context store is instead an **A/B pair of whole sectors** at +`0x01FC0000`, immediately below the ephemeris store: erase the inactive sector, +write the payload, write the header last. A save torn by power loss leaves a +sector whose header never validates, so mount falls back to the previous +generation. + +### The wear policy + +Almanac pages are rebroadcast continuously, so almost every save carries bytes +identical to what is already stored. The store compares a CRC **before** +touching the flash and returns without erasing or programming when nothing +changed. Erase count therefore follows how often the constellation publishes +new data - roughly daily - not how often the save timer fires. The host test +runs 50 repeat saves and asserts zero erases and zero programs. + +> The first implementation of this did not actually achieve it: the payload +> also carried the live WLS position and the receiver time, so the CRC differed +> on every save and the flash was erased every 60 s. See section 8.9.3. + +### Cold start, stale data, and a moved receiver + +Nothing restored here is trusted for navigation on its own: + +* Every record is **re-parsed through the live decoder**, so a corrupt or stale + page is rejected exactly as a bad broadcast would be. +* The almanac only biases *how often* a PRN is searched. It cannot make a + satellite unreachable (section 8.7). +* The ionosphere model is replaced the moment page 18 is heard again. +* The position is only a **seed** for the visibility prediction, used until the + first real fix replaces it. While the seed is unconfirmed the elevation mask + is widened to -25 degrees so a receiver carried a long way between power + cycles still searches what has risen at the new site (section 8.9.5); the + mask closes back to -5 degrees at the first fix. +* An almanac too old to propagate produces **no** prediction rather than a + wrong one, and a health byte is only believed while the orbit it arrived + with is still usable (sections 8.9.2 and 8.9.5). + +Ephemeris is deliberately *not* part of this store: it expires in hours and +already has its own store with a curve-fit check. + +### Board results, 2026-08-08 + +First boot wrote `generation=1` then 2, 3 as pages arrived, with `iono=1` and +`position=1`. After a reload: + +``` +[NAV] navigation context restored: almanac=6 iono=1 position=1 generation=3 +``` + +`searchable` fell from 32 to 27 within seconds of boot instead of after 12.5 +minutes, and `skipped=1` confirmed the no-write path. + +### A bug this caught on hardware + +The first build restored the ionosphere page into the parsed model but not into +the raw-word cache. The periodic save rebuilds its payload from that cache, so +the next save wrote the page back as **absent** - wiping the stored model on the +first save after every reboot and defeating the persistence entirely. It showed +up as `iono=0` in the save telemetry one line after a restore had reported +`iono=1`. Fixed in `gps_lnav_restore_iono_page()`, with a round-trip regression +in `tests/test_gps_lnav_iode.c` covering both the iono and almanac paths. + +## 8.9 Audit of the persistence work, 2026-08-09 + +Persistence changed the lifetime of the almanac, the ionosphere page and the +last position from *one power cycle* to *weeks*. Three separate pieces of +arithmetic were only ever correct over the shorter lifetime, and one of them +was a latent hang with a date on it. All five findings below reproduce in +`tests/test_gps_nav_context_audit.c`, which prints the before and after values +in the same run. + +### 8.9.1 A hang armed for GPS week 2433 (severe) + +`parse_subframe()` lifted the broadcast WNa - the eight LSBs of the almanac +reference week - against `gps_ephems.toe.week`. **Nothing in the receiver ever +writes that field**: LNAV broadcasts Toe as a second-of-week only, and the +field was read exactly once, here. Lifting against the zero it held put the +result 256 weeks below the receiver for every WNa above 128, which in unsigned +arithmetic underflowed: + +``` +the previous lift gave week 4294967169 for WNa 129 (receiver week 383) +``` + +`gps_almanac_satellite_ecef()` then reduced `tk` with `while (tk < -302400) tk ++= 604800;` - **4.29e9 iterations**, measured at 4.2 s per satellite on a 5 GHz +x86 and therefore hours on a 50 MHz in-order Rocket core, times 32 satellites, +inside a function called every 10 s from the foreground service loop. That +loop also drives acquisition scheduling, the flash service and status logging, +so the receiver would simply stop. + +GPS week 2431 has WNa 127, so this had not fired yet. It arms at **week 2433 +(2026-08-23)** and then for 127 weeks out of every 256. Subframe 5 page 25 +arrives once per 12.5 minutes, so it would have hit within 12.5 minutes of +every boot - and the poisoned week would have been written to flash. + +Three independent fixes, because one is not enough for a fault of this shape: + +1. The lift uses `tow.week` (the week the receiver actually resolves), in + signed arithmetic, with a symmetric comparison; with no week to lift + against the reference is left unset rather than guessed. +2. `gps_almanac_set_reference()` refuses a week that cannot be a GPS week, so + neither the air nor a flash record written by an older build can install + one. +3. The propagation is closed form. There is no loop left to run away. + +### 8.9.2 The half-week wrap made stale almanacs confidently wrong + +`gps_almanac_satellite_ecef()` wrapped `tk` into the half week +unconditionally, citing IS-GPS-200 20.3.3.5.2.3. That paragraph is a *week +crossover* correction - so a toa late in one week and a receiver time early in +the next give a small positive age - not a general reduction. Applied as one it +aliases, and a GPS week is **14.0384 orbital periods rather than 14**, so +folding one away moves the satellite about 14 degrees along its orbit. + +Checked against an independent Python implementation of Table 20-IV +(`scratchpad/alm_ref.py` reproduced in the test): one week of aliasing displaces +the reference satellite by **5161 km**, and from a receiver at 40 N a satellite +genuinely 5.10 degrees *below* the horizon is reported 5.67 degrees *above* it - +opposite sides of the -5 degree acquisition mask. + +Collecting an almanac live, this was unreachable: a page just received is never +half a week old. Reading one back from flash, it is the normal case. + +Now: `tk` is the true elapsed time, the crossover guess lives in +`gps_almanac_assumed_week()` at the one point where a week genuinely has to be +guessed (before page 25 has been heard), and anything beyond +`GPS_ALMANAC_MAX_AGE_SEC` (14 days) yields **no prediction at all**, leaving +the PRN fully searchable. Fail-safe direction: a stale almanac costs a full +sweep, never a demoted satellite. + +The reference vector in `tests/test_gps_almanac.c` was regenerated - the old one +had been produced with the wrap in place, so it had been encoding the bug. + +### 8.9.3 The wear policy never engaged + +The store's CRC comparison only helps if the bytes offered are stable while +nothing meaningful has changed. They were not: the payload carried the raw WLS +position, which moves by metres every epoch, and `position_tow_sec` / +`position_week`, which move always and which **nothing on the restore path ever +read**. So every periodic save was a fresh CRC and therefore an erase. The +board's own log shows it - generation 12 to 17 across one short capture with a +single skipped write. + +``` +24 h stationary, 1440 saves: taking every fix -> 1440 erases, 0.4 years to 100k cycles +24 h stationary, 1440 saves: held seed -> 1 erases (1439 skipped), 548 years to 100k cycles +``` + +1440 erase cycles a day against 100k cycles shared over two sectors is about +**139 days to wear-out**, to persist a seed that is read once, at the next +boot. + +The position is now *held*: refreshed only when the receiver has moved more +than `GPS_NAV_CONTEXT_POSITION_EPSILON_M` (5 km) and no more often than +`GPS_NAV_CONTEXT_POSITION_REFRESH_TICKS` (1 h). A look angle at 20200 km moves +by ~0.003 degrees per kilometre of receiver error, so kilometres are free and +metres are noise. A moving platform still updates its seed, at a cadence set by +how far it has gone rather than by how long the receiver has been on. + +### 8.9.4 A stale flash ephemeris could outrank the live broadcast + +Toe is a second-of-week, so `gps_ephemeris_supersedes()` orders two records +with the signed half-week rule. That is exact between records from the same era +and **inverted** once the gap exceeds half a week. 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 later cold start restores it again. + +Records read back from flash are now marked `restored` and adopted through +`gps_ephemeris_cache_store_restored()`. A restored record is a candidate, not +an incumbent: the first broadcast for that PRN replaces it outright, whatever +the Toe arithmetic says, and the flag clears so ordinary Toe ordering resumes. +A restored record can never displace something already learned this run. + +### 8.9.5 A moved receiver, and stale health + +Two smaller ones, both consequences of the same lifetime change: + +* A health byte from a weeks-old almanac was still demoting satellites. Health + only ever demotes, so it must not be trusted further than the orbit it came + with; the health test now sits behind the propagation gate. +* The persisted seed says where the receiver *was*. While it is still the only + position available, the elevation mask widens to + `GPS_ACQ_VISIBILITY_SEED_MASK_ELEVATION_DEG` (-25 degrees), which covers a + couple of thousand kilometres of unknown migration and still rules out the + far side of the earth. Measured in the test, a 1600 km migration between + power cycles demoted **4 of the 16 satellites actually up** at the new site + under the -5 degree mask, and **none** under -25. + +### What was checked and found correct + +* The Klobuchar ionospheric delay in `NavEquations.c` matches IS-GPS-200 + 20.3.3.5.2.5 term for term, including the semicircle units, the +/-0.416 + latitude clamp, the 72000 s period floor and the |x| < 1.57 cutoff. +* All almanac field positions, scale factors and sign extensions, including the + split af0, verified against the ICD and against the Python reference. +* The A/B sector layout, the header-last ordering, the power-loss behaviour and + the CRC rejection all hold under the simulated NOR (bit-clear-only + programming, 256-byte page limit, truncated writes). +* Flash regions do not overlap: navigation context at 0x01FC0000-0x01FDFFFF, + ephemeris store at 0x01FE0000-0x01FFFFFF, both above the application at + 0x01800000. +* Sanitizer clean (ASan + UBSan) across the almanac, LNAV, cache, store and + audit suites. + +One hardening item came out of the sanitizer build rather than from reading: +`gps_lnav_almanac_words[sv_id - 1u]` was bounded only because +`gps_almanac_store_page()` re-derives the same field from the same words and +returns false outside 1..32. GCC was right to object - that is an invariant one +refactor away from a 40 byte write past a static array - and the range test is +now stated at the call site. + +## 8.10 Cold start, channel churn, and what reaches the WLS, 2026-08-09 + +Second audit pass, aimed at the measurement path rather than at persistence: +does a satellite appearing or disappearing, or a coalesced interrupt, or the +32-bit sample counter wrapping, ever hand the WLS something inconsistent? +Regressions in `tests/test_gps_pvt_consistency.c`. + +### 8.10.1 Cold start does search everything + +Confirmed by code path and by test. `gps_refresh_acq_visibility()` forces the +mask to all ones whenever `gps_receiver_time_now()` fails, and at power-on it +always does - receiver time arrives only once a satellite is tracked and its +handover word decoded. So the first sweep is all 32 PRNs, and the restored +almanac starts aiming only after the first satellite is up. There is no RTC, so +this is not improvable: the almanac cannot be evaluated without time. + +An empty mask - what a corrupt or absent flash almanac produces - degrades to +the same blind sweep rather than to silence. + +### 8.10.2 The ephemeris age check had a whole-week blind spot (serious) + +LNAV broadcasts Toe as a second-of-week. `gps_ephemeris_toe_offset_sec()` +compared it against the receiver's second-of-week with the signed half-week +rule and **never looked at a week number at all** - `toe.week` existed but +nothing wrote it. The age was therefore computed modulo one week, so a record +exactly N weeks old, evaluated at a similar time of week, read as brand new: + +``` + 50 minutes old -> ok + 2h13m old (past fit) -> age + exactly 1 week old -> ok <-- accepted as current + 4 weeks old -> ok <-- accepted as current +``` + +Unreachable while ephemeris lives in RAM and expires in hours. Directly +reachable once it is persisted to flash: switch the receiver off and on again a +week later at a similar hour and it accepts its own week-old orbits, worth +kilometres of position error with nothing to flag it. This is the same class of +defect as the almanac week bug in section 8.9.1 - a modulo reduction standing +in for a week number - in a second place. + +Fixed at the source: `gps_resolve_toe_week()` reconstructs the week at commit +time from the subframe 1 week number and the handover word of the subframe that +carried Toe, resolving both week-crossover directions (Toe early in the +following week, and a transmission that crossed into it). The age check then +uses the true elapsed time with no reduction. A record whose week cannot be +resolved gets no age at all, which reads as a rejection everywhere - the safe +direction, and what old flash records written before this change will do. + +### 8.10.3 Reference selection screened only part of the set + +`gps_pps_select_reference()` returned as soon as it found the sticky reference +PRN, leaving any non-finite capture time *after* it in the array unexamined. So +whether a corrupt measurement was caught depended on where the reference PRN +happened to sit. Neither branch could produce a wrong fix - a non-finite +pseudorange fails the WLS build and the epoch is lost either way - but the two +took different paths with different diagnostics. It now screens the whole set +before choosing anything. + +### 8.10.4 Channel churn: verified correct + +Each of these is now a regression test. + +| Situation | Behaviour | +|---|---| +| Satellite acquired mid-epoch | `expected_mask` is frozen when the first event of a period arrives, so the new channel's push returns INVALID and it joins the **next** epoch. Its measurement never enters the current snapshot. | +| Freshly seeded channel | `trk_seed_unconfirmed` keeps it out of `ready_mask` until its first epoch validates, so an unproven phase seed cannot hold up an epoch. | +| Satellite lost mid-epoch | `gps_process_tracking_drop()` removes it from `expected_mask`, `received_mask` and the snapshot mask, and publishes immediately if what remains is complete - rather than stalling to the deadline. | +| Loss takes the set below four | Publication refuses and the whole epoch is discarded. A three-satellite set is never offered. | +| Channel goes silent without a drop event | Excluded after `GPS_NAV_SILENT_CHANNEL_EPOCHS` misses - but never if that would take the expected set below four, where waiting beats excluding. | +| Channel reassigned to a new PRN | The snapshot holds a **copy** of the ephemeris and the PRN per channel, and the collect step re-checks `eph->SVID == epoch->prn[ip]`. A reassigned channel cannot contribute the previous PRN's orbit. Measurements carry their PRN too; a mismatch yields a NaN C/N0 rather than a wrong weight. | +| Disconnected but still reported locked | Not possible: `gps_nav_ready_channel_mask()` reads BUSY and LOCK from `STATUS_REG` live at every push, alongside `tracking_allocated` and the nav-solve interrupt enable. | +| Coalesced interrupts | `gps_nav_capture_sequence_consistent()` requires the capture-counter delta and the sequence delta to agree. If they do not, the event is refused and only the transport metadata is rebased - it never enters an epoch. Missing epochs are counted, never synthesised. | +| Duplicate / stale / out-of-window capture | Rejected as DUPLICATE, STALE and OUTLIER respectively; none of them touches the snapshot. | +| Four of five channels coherent | **Not** published as a four-satellite fix. Publication requires the whole frozen set to be coherent, because a solvable subset would make the satellite geometry depend on interrupt timing - the original source of large raw-position jumps. | + +### 8.10.5 The 32-bit sample counter wraps every 262 seconds + +At 16.368 MHz a `uint32_t` sample counter wraps every 262 s, in the middle of +normal tracking. `gps_nav_epoch_unwrap_sample()` (moved out of `gps_ip.c` so it +is host-testable) lifts every accepted channel onto the reference's axis by +signed modulo-2^32 distance. Building one epoch away from the wrap and again +straddling it gives pseudoranges identical to **3.3 micrometres**, which is +double rounding. The epoch selector's own comparisons are modular too, so a +coherent epoch is not rejected once every 262 s. + +### 8.10.6 The remaining exposure, stated plainly + +`GPS_NAV_ENABLE_FDE` is **0**: fault detection and exclusion is compiled out. +Even enabled it requires seven satellites and only runs when the full solve +*succeeded* but failed chi-square, so a structurally broken row - non-finite, +degenerate range, zero weight - can never be excluded; it fails the whole +epoch. + +With five or more satellites that is safe but costly: `solution_valid` requires +`chi_square_pass`, so a bad measurement yields **no fix** rather than a wrong +one. With exactly four satellites there are zero residual degrees of freedom, +no chi-square statement is possible, and a bad measurement produces a fix that +is wrong with nothing to detect it. That is algebraically unavoidable at four +satellites and is already documented at the `solution_valid` assignment. + +The most likely source of such a measurement was a satellite low in the sky. +That part is now fixed; see section 8.11. There is still **no elevation mask +anywhere on the navigation path** - nothing is excluded on elevation, by design, +because with nine channels and a four-satellite minimum excluding is often worse +than including a discounted measurement. + +## 8.11 The measurement model: what one satellite is allowed to do, 2026-08-09 + +Three quantities were being conflated, and only one of them is a bias: + +* the **corrections**, `tropo()` and `klobuchar_delay_meters()`, subtracted from + the observation to remove a real physical delay; +* the **sigma**, `gps_nav_measurement_sigma_m()`, which never shifts anything - + it only says how much to trust a satellite; +* the **WLS clock bias**, the fourth unknown, which is solved for. + +Two defects, both in the first two. Regressions in +`tests/test_gps_measurement_model.c`. + +### 8.11.1 The weighting floor stopped the weighting model working + +The sigma model itself is sound: variance is `sigma_URA^2 + sigma_elevation^2 + +sigma_CN0^2`, weight is `1/variance`, and the carrier-to-noise law is +physically right - DLL jitter goes as the inverse square root of C/N0 in linear +power, so 20 dB is exactly a factor of ten, which is what +`3.0 * 10^((45 - CN0)/20)` gives. + +The defect was a clamp that stopped it applying: + +```c +const double min_elevation_deg = 10.0; +if (bounded_elevation_deg < min_elevation_deg) bounded_elevation_deg = 10.0; +``` + +That pinned the elevation term at `3/sin(10 deg)` = 17.3 m. The model's own +answer at 5 degrees is 34.4 m and at 2 degrees 86 m, so a satellite low enough +to be carrying most of the error was trusted 4 to 25 times more than this same +model said it deserved. Nothing was ever *excluded* by that floor - it only made +the mapping stop working exactly where it mattered. + +The floor is now `GPS_NAV_WEIGHT_MIN_ELEVATION_DEG` = 3 degrees. It cannot be +removed entirely: `1/sin` diverges at the horizon, and a zero weight is not a +discounted satellite but a failed epoch, because +`least_squares_build_postfit()` refuses a non-positive weight. Three degrees +leaves the term at 57 m, some 350 times lighter than zenith. + +Measured end to end on a six-satellite geometry, injecting 30 m of uncorrected +delay on one satellite and taking the displacement between two solves: + +``` + 30 m of uncorrected delay moves the fix by 4.18 m on the 4 deg satellite, + 48.50 m on the 72 deg one + the 10 deg floor gave that satellite sigma 17.9 m instead of 43.3 m: + 5.8x the weight +``` + +That asymmetry is the point. The solve is now far less sensitive to a bias on a +satellite it should not trust than to the same bias on one it should; under the +old floor the two were within a factor of six of each other. (A near-zenith +satellite moving the fix by more than its own bias is normal: its line of sight +is nearly degenerate with the clock-bias column, so the error splits into +vertical position and clock.) + +### 8.11.2 The corrections switched themselves off below the horizon + +`tropo()` was called only for `elv > 0.0` and `klobuchar_delay_meters()` +returned zero for `elevation_deg <= 0.0`. So a satellite crossing the horizon - +setting, or arriving by reflection - **lost its entire atmospheric correction in +one step**, and that whole magnitude stayed in its measurement as an +uncorrected bias. At this site the horizon tropospheric delay alone is 78.8 m: + +``` + troposphere at +0.05/0.00/-0.05 deg: 77.22 / 78.77 / 78.77 m + -> below the horizon it holds 78.8 m instead of dropping to 0 +``` + +Both are now evaluated with elevation clamped at the horizon +(`least_squares_atmosphere_elevation_deg()`). That extrapolation +under-corrects, because the true path below the horizon is longer still, but it +is far closer than zero and it makes the measurement model continuous - which +matters more, since a discontinuity means the same satellite changes the fix by +tens of metres between consecutive epochs for no physical reason. + +The extra uncertainty of extrapolating is charged rather than assumed away: +`GPS_NAV_BELOW_HORIZON_SIGMA_M` (50 m) is added in quadrature at or below the +horizon, so such a satellite is discounted for the reflection it probably +arrived by as well as for being low. + +A non-finite elevation now also takes the floor rather than being silently +treated as a 10 degree satellite - unknown is not the same as good. + +## 9. Known remaining gaps + +* Recovery-time validation for the two fixes in section 8.5 is outstanding and + needs a 7-8 satellite sky. +* Almanac-directed acquisition (section 8.7) is verified on the host suite; + its effect on reacquisition time on hardware still needs a drop test. +* Recovery after a total RF outage measured ~43 s on 2026-08-08, of which about + 20 s was spent with satellites **tracked but not `ready`** - `how_reject`, + `eph_reject`, `incoherent` and the assembly counters were all flat, so the + gate is `gps_node_ready_for_position()`. A cold start with a warm cache + reaches a fix in ~6 s with `eph_miss=0`, so the restore path itself is fast. + The untested hypothesis is that a *known* receiver time makes + `gps_ephemeris_cache_restore_at()` apply the full curve-fit window and reject + an aged cached ephemeris, forcing a fresh 18-30 s subframe decode, whereas at + cold start time is unknown and only static checks apply. `eph_hit`/`eph_miss` + are now in the epoch funnel; one more drop test settles it. +* The acquisition **peak metric of a failed search is unavailable to + firmware**. `acqusition_wrapper.sv` latches it only on success + (`peak_metric_reg_n = acq_stat_p ? peak_metric_syned : peak_metric_reg_p;`) + and clears it on every `start_acq`, so a failure always reads 0. That makes + "no satellite there" indistinguishable from "satellite present but below the + fixed threshold" - `PEAK_THRESHOLD = 512`, a peak/second-peak ratio of 2.00, + hard-coded in `common/definitions.sv` with no firmware control. Latching it + unconditionally is a one-line RTL change on an existing capture path, and + would let the sensitivity limit be measured rather than inferred. Not made: + RTL changes need explicit approval. +* The 68 ms `STARTOFFSET` datum is retained by decision, not by omission + (section 4.1). It is not a defect and not scheduled for replacement. +* Board validation covers the seed path, flash persistence, the format path and + compaction (sections 8.1-8.3). Not yet covered on hardware: the multi-minute + outage cases, and any natural drop long enough to age a PRN context out. +* **Firmware and bitstream must be rebuilt together.** The RTL change adds one + register and one state-machine branch to + `tracking_wrapper`/`nav_solve_timer`, and the Genesys2 bitstream has not been + regenerated, so the current `.bit`/`.mcs` in `targets/` predate it. Writing + `NAV_SOLVE_PHASE_SEED` against an older bitstream hits the wrapper's `default` + decode case, which asserts `PSLVERR`; `TLToAPB` maps that to `denied` on the + write response, and Rocket can take a store access fault. This is *not* + benign. Build with `GPS_WARM_SEED_ENABLE=0` to produce firmware for a + bitstream without the register — the seeding path is then compiled out and + every channel takes the subframe path. +* Flash persistence now has board evidence (section 8.1): the 4-byte-address + opcodes and status polling in `bsp/spi_flash.c` work against the real + S25FL256S. Sector erase has not been exercised on hardware, because no + compaction has happened yet and the store was already formatted. +* Channel generation IDs are not implemented. The FIFO event queue plus + foreground-only reassignment closes the ordinary stale-IRQ window, but there + is no explicit generation stamp proving a queued event predates a + reassignment. +* The cached ephemeris carries the raw modulo-1024 LNAV week in `tow.week` + until the first nav-solve overwrites it with the extended week. Validity does + not depend on it (section 6 uses the half-week rule), but flash persistence + will need the extended week stored explicitly. diff --git a/soc/software/gps_system/header/gps/NavEquations.h b/soc/software/gps_system/header/gps/NavEquations.h index e979e35..25ff516 100644 --- a/soc/software/gps_system/header/gps/NavEquations.h +++ b/soc/software/gps_system/header/gps/NavEquations.h @@ -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); diff --git a/soc/software/gps_system/header/gps/gps_acq_manager.h b/soc/software/gps_system/header/gps/gps_acq_manager.h index 4f82eaf..da1199b 100644 --- a/soc/software/gps_system/header/gps/gps_acq_manager.h +++ b/soc/software/gps_system/header/gps/gps_acq_manager.h @@ -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); diff --git a/soc/software/gps_system/header/gps/gps_almanac.h b/soc/software/gps_system/header/gps/gps_almanac.h new file mode 100644 index 0000000..a0537d5 --- /dev/null +++ b/soc/software/gps_system/header/gps/gps_almanac.h @@ -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 +#include + +#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 diff --git a/soc/software/gps_system/header/gps/gps_ephemeris_cache.h b/soc/software/gps_system/header/gps/gps_ephemeris_cache.h index 6f66c84..26b205e 100644 --- a/soc/software/gps_system/header/gps/gps_ephemeris_cache.h +++ b/soc/software/gps_system/header/gps/gps_ephemeris_cache.h @@ -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 diff --git a/soc/software/gps_system/header/gps/gps_ephemeris_store.h b/soc/software/gps_system/header/gps/gps_ephemeris_store.h new file mode 100644 index 0000000..2881883 --- /dev/null +++ b/soc/software/gps_system/header/gps/gps_ephemeris_store.h @@ -0,0 +1,127 @@ +#ifndef GPS_EPHEMERIS_STORE_H +#define GPS_EPHEMERIS_STORE_H + +#include +#include + +#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 diff --git a/soc/software/gps_system/header/gps/gps_ephemeris_validity.h b/soc/software/gps_system/header/gps/gps_ephemeris_validity.h new file mode 100644 index 0000000..0a22e6c --- /dev/null +++ b/soc/software/gps_system/header/gps/gps_ephemeris_validity.h @@ -0,0 +1,98 @@ +#ifndef GPS_EPHEMERIS_VALIDITY_H +#define GPS_EPHEMERIS_VALIDITY_H + +#include +#include + +#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 diff --git a/soc/software/gps_system/header/gps/gps_interrupt_handlers.h b/soc/software/gps_system/header/gps/gps_interrupt_handlers.h index 00e55ca..6e38e8d 100644 --- a/soc/software/gps_system/header/gps/gps_interrupt_handlers.h +++ b/soc/software/gps_system/header/gps/gps_interrupt_handlers.h @@ -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); diff --git a/soc/software/gps_system/header/gps/gps_ip_defines.h b/soc/software/gps_system/header/gps/gps_ip_defines.h index e1ceb9c..ab22efd 100644 --- a/soc/software/gps_system/header/gps/gps_ip_defines.h +++ b/soc/software/gps_system/header/gps/gps_ip_defines.h @@ -8,6 +8,7 @@ #include #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); diff --git a/soc/software/gps_system/header/gps/gps_nav_context_store.h b/soc/software/gps_system/header/gps/gps_nav_context_store.h new file mode 100644 index 0000000..46239e4 --- /dev/null +++ b/soc/software/gps_system/header/gps/gps_nav_context_store.h @@ -0,0 +1,127 @@ +#ifndef GPS_NAV_CONTEXT_STORE_H +#define GPS_NAV_CONTEXT_STORE_H + +#include +#include + +#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 diff --git a/soc/software/gps_system/header/gps/gps_nav_epoch.h b/soc/software/gps_system/header/gps/gps_nav_epoch.h index c009d93..dbb5cc7 100644 --- a/soc/software/gps_system/header/gps/gps_nav_epoch.h +++ b/soc/software/gps_system/header/gps/gps_nav_epoch.h @@ -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; diff --git a/soc/software/gps_system/header/gps/gps_pps.h b/soc/software/gps_system/header/gps/gps_pps.h index 8e5eb5c..be432ee 100644 --- a/soc/software/gps_system/header/gps/gps_pps.h +++ b/soc/software/gps_system/header/gps/gps_pps.h @@ -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, diff --git a/soc/software/gps_system/header/gps/gps_time_anchor.h b/soc/software/gps_system/header/gps/gps_time_anchor.h index f0877c6..ec96e99 100644 --- a/soc/software/gps_system/header/gps/gps_time_anchor.h +++ b/soc/software/gps_system/header/gps/gps_time_anchor.h @@ -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. */ diff --git a/soc/software/gps_system/src/gps/NavEquations.c b/soc/software/gps_system/src/gps/NavEquations.c index 4d15bf2..f8f3f03 100644 --- a/soc/software/gps_system/src/gps/NavEquations.c +++ b/soc/software/gps_system/src/gps/NavEquations.c @@ -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; diff --git a/soc/software/gps_system/src/gps/gps_acq_manager.c b/soc/software/gps_system/src/gps/gps_acq_manager.c index 99c2127..d028fbc 100644 --- a/soc/software/gps_system/src/gps/gps_acq_manager.c +++ b/soc/software/gps_system/src/gps/gps_acq_manager.c @@ -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; diff --git a/soc/software/gps_system/src/gps/gps_almanac.c b/soc/software/gps_system/src/gps/gps_almanac.c new file mode 100644 index 0000000..df00e6b --- /dev/null +++ b/soc/software/gps_system/src/gps/gps_almanac.c @@ -0,0 +1,470 @@ +#include "gps/gps_almanac.h" + +#include +#include + +#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; +} diff --git a/soc/software/gps_system/src/gps/gps_ephemeris_cache.c b/soc/software/gps_system/src/gps/gps_ephemeris_cache.c index 66bd268..f640fd0 100644 --- a/soc/software/gps_system/src/gps/gps_ephemeris_cache.c +++ b/soc/software/gps_system/src/gps/gps_ephemeris_cache.c @@ -3,6 +3,8 @@ #include #include +#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; +} diff --git a/soc/software/gps_system/src/gps/gps_ephemeris_store.c b/soc/software/gps_system/src/gps/gps_ephemeris_store.c new file mode 100644 index 0000000..b4c13ae --- /dev/null +++ b/soc/software/gps_system/src/gps/gps_ephemeris_store.c @@ -0,0 +1,529 @@ +#include "gps/gps_ephemeris_store.h" + +#include +#include + +#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; +} diff --git a/soc/software/gps_system/src/gps/gps_ephemeris_validity.c b/soc/software/gps_system/src/gps/gps_ephemeris_validity.c new file mode 100644 index 0000000..936aba4 --- /dev/null +++ b/soc/software/gps_system/src/gps/gps_ephemeris_validity.c @@ -0,0 +1,231 @@ +#include "gps/gps_ephemeris_validity.h" + +#include +#include + +#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; +} diff --git a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c index 07644a0..b50801b 100644 --- a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c +++ b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c @@ -1,12 +1,16 @@ #include "gps/linked_list.h" #include "gps/gps_acq_manager.h" #include "gps/gps_ephemeris_cache.h" +#include "gps/gps_ephemeris_store.h" +#include "gps/gps_nav_context_store.h" +#include "gps/gps_ephemeris_validity.h" #include "gps/gps_interrupt_handlers.h" #include "gps/Navigation.h" #include "gps/gps_nav_epoch.h" #include "gps/gps_pps.h" #include "gps/gps_time_anchor.h" #include +#include #include #include #include @@ -62,10 +66,50 @@ static gps_tracking_shadow_t trk_shadow[GPS_TRCK_IP_COUNT]; static gps_tracking_measurement_t gps_latest_tracking_measurement[GPS_TRCK_IP_COUNT]; +/* + * PRN-level navigation context, deliberately outliving the physical tracking + * channel. It holds the last confirmed nav-solve epoch for that satellite: + * transmit time, the PPS capture counter and the RF sample latch, all captured + * by one hardware pulse. A returning PRN uses it to seed the tracking IP's + * epoch phase instead of waiting for a subframe to be decoded again. + */ +typedef struct { + bool valid; + int week; + double tow_sec; + uint64_t pps_counter; + uint32_t raw_sample; + double corrected_sample; + uint64_t tick; +} gps_prn_nav_context_t; + +static gps_prn_nav_context_t gps_prn_context[GPS_PRN_COUNT]; +/* Channel-level transient warm-start state. */ +/* CLINT timestamps for warm reacquisition latency measurement. */ +static uint64_t gps_prn_drop_mtime[GPS_PRN_COUNT]; +static uint64_t trk_assign_mtime[GPS_TRCK_IP_COUNT]; +/* Consecutive epochs a channel was expected in but did not report. */ +static uint32_t trk_nav_solve_miss[GPS_TRCK_IP_COUNT]; +static bool trk_seed_requested[GPS_TRCK_IP_COUNT]; +static bool trk_seed_unconfirmed[GPS_TRCK_IP_COUNT]; +static unsigned int trk_seed_prn[GPS_TRCK_IP_COUNT]; + static volatile unsigned char satellite_tracking_ip[GPS_PRN_COUNT]; static gps_acq_result_t satellite_acq_result[GPS_PRN_COUNT]; static gps_acq_manager_t gps_acq_manager; static gps_ephemeris_cache_t gps_ephemeris_cache; +#if (GPS_EPHEMERIS_FLASH_ENABLE == 1) +static gps_ephemeris_store_t gps_ephemeris_store; +static bool gps_ephemeris_store_ready; +/* + * Flash writes are deferred out of the event path. A 64 KiB sector erase on + * this device runs for hundreds of milliseconds and would stall nav-solve + * processing; page programs are ~1 ms. The background task drains one PRN per + * pass, so at most one record is written between event drains. + */ +static volatile uint32_t gps_ephemeris_flash_pending_mask; +static unsigned int gps_ephemeris_flash_words[GPS_PRN_COUNT][3][10]; +#endif static gps_isr_event_t gps_event_queue[GPS_EVENT_QUEUE_SIZE]; static volatile unsigned int gps_event_head = 0; @@ -81,6 +125,17 @@ static gps_nav_epoch_snapshot_t gps_nav_completed_snapshot; static bool gps_nav_completed_snapshot_pending; static volatile gps_runtime_diagnostics_t gps_runtime_diagnostics; static uint64_t gps_last_acq_status_log_tick = 0u; +/* + * Coarse receiver GPS time, refreshed from every accepted nav-solve epoch and + * extrapolated with the CLINT systick between epochs. It only has to be good + * enough to place an ephemeris inside or outside a multi-hour curve fit + * window, so systick-rate drift is irrelevant here. The navigation solution + * keeps using the per-channel transmit times, not this value. + */ +static bool gps_receiver_time_valid = false; +static int gps_receiver_time_week = 0; +static double gps_receiver_time_tow_sec = 0.0; +static uint64_t gps_receiver_time_tick = 0u; static unsigned int gps_external_irq_id = 0u; static gps_external_irq_handler_t gps_external_irq_handler = NULL; @@ -220,9 +275,98 @@ static bool gps_copy_latest_tracking_measurement( return true; } +/* + * Attribute a finished epoch to the channels that did and did not report. + * expected_mask is latched when the epoch opens, so a channel that cannot emit + * holds every epoch it appears in to the deadline; counting the misses is what + * lets it be dropped from the expected set instead. + */ +typedef enum { + GPS_EPOCH_ACCT_COMPLETE = 0, + GPS_EPOCH_ACCT_DEADLINE, + GPS_EPOCH_ACCT_RESTART +} gps_epoch_account_reason_t; + +static void gps_nav_epoch_account(uint32_t expected_mask, + uint32_t received_mask, + uint64_t minimum_counter, + uint64_t maximum_counter, + gps_epoch_account_reason_t reason) +{ + uint32_t missing = expected_mask & ~received_mask; + + if(reason == GPS_EPOCH_ACCT_DEADLINE) + { + gps_runtime_diagnostics.nav_epoch_deadline_discard_total++; + } + else if(reason == GPS_EPOCH_ACCT_RESTART) + { + gps_runtime_diagnostics.nav_epoch_restart_discard_total++; + } + else if(maximum_counter >= minimum_counter) + { + /* Only a complete epoch gives the true inter-satellite spread; a + * partial one is bounded by whoever happened to report. */ + uint64_t spread = maximum_counter - minimum_counter; + uint32_t spread_ticks = (spread > 0xffffffffu) ? + 0xffffffffu : (uint32_t)spread; + gps_runtime_diagnostics.nav_epoch_spread_last_ticks = spread_ticks; + if(spread_ticks > + gps_runtime_diagnostics.nav_epoch_spread_max_ticks) + { + gps_runtime_diagnostics.nav_epoch_spread_max_ticks = + spread_ticks; + } + } + + gps_runtime_diagnostics.nav_epoch_last_expected_mask = expected_mask; + gps_runtime_diagnostics.nav_epoch_last_missing_mask = missing; + gps_runtime_diagnostics.nav_epoch_blocked_mask |= missing; + + for(unsigned int i = 0u ; i < GPS_TRCK_IP_COUNT ; i++) + { + uint32_t bit = 1u << i; + if((received_mask & bit) != 0u) + { + trk_nav_solve_miss[i] = 0u; + continue; + } + if((missing & bit) == 0u) + { + continue; + } + trk_nav_solve_miss[i]++; + gps_runtime_diagnostics.nav_epoch_missing_count[i]++; + if(trk_nav_solve_miss[i] != GPS_NAV_SILENT_CHANNEL_EPOCHS) + { + continue; + } + + /* Crossing the threshold: this channel is now excluded from the + * expected set by gps_nav_ready_channel_mask(). Its hardware timer is + * almost certainly idle, which is exactly the state that accepts a + * phase seed, so ask for one rather than waiting for a subframe. A + * wrong guess is free: the IP refuses a seed unless it is idle. */ + gps_runtime_diagnostics.nav_epoch_silenced_total++; + unsigned int prn = trck_ips[i]->PRN_NUMBER; + if(gps_prn_valid(prn) && gps_prn_context[prn].valid && + gps_frames_parsed[i].gps_ephems.ephemeris_valid && + !trk_seed_requested[i]) + { + trk_seed_prn[i] = prn; + trk_seed_requested[i] = true; + gps_runtime_diagnostics.nav_epoch_reseed_total++; + } + GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, + "channel silent, excluded from epoch: trk=%u prn=%u expected=0x%x missing=0x%x\r\n", + i, prn, expected_mask, missing); + } +} + static uint32_t gps_nav_ready_channel_mask(void) { - uint32_t ready_mask = 0u; + uint32_t eligible_mask = 0u; + uint32_t active_mask = 0u; for(unsigned int i = 0u ; i < GPS_TRCK_IP_COUNT ; i++) { uint32_t status = trck_ips[i]->STATUS_REG; @@ -231,12 +375,564 @@ static uint32_t gps_nav_ready_channel_mask(void) bool locked = (status & (1u << GPS_TRCK_LOCK_POS)) != 0u; bool nav_irq_enabled = (control & (1u << GPS_TRCK_NAV_SOLVE_INTR_EN_POS)) != 0u; - if(tracking_allocated[i] && busy && locked && nav_irq_enabled) + /* A freshly seeded channel is emitting epochs but its time anchor has + * not been checked against one yet. Keep it out of the expected set so + * an unproven seed cannot hold up a coalescing epoch; it joins as soon + * as its first epoch validates. */ + if(!tracking_allocated[i] || !busy || !locked || !nav_irq_enabled || + trk_seed_unconfirmed[i]) { - ready_mask |= 1u << i; + continue; + } + eligible_mask |= 1u << i; + /* Silent channels are excluded until they emit again. The miss count + * is cleared in the nav-solve event handler, not here, because an + * excluded channel's events never reach the epoch assembly. */ + if(trk_nav_solve_miss[i] < GPS_NAV_SILENT_CHANNEL_EPOCHS) + { + active_mask |= 1u << i; } } - return ready_mask; + + /* + * Excluding a silent channel must never take the expected set below the + * WLS minimum. Observed on the board: with four satellites, dropping one + * left three and made a solution impossible, where waiting for it to + * recover would have produced a fix. Below the minimum, waiting is + * strictly better than excluding. + */ + if(gps_mask_count(active_mask) >= GPS_NAV_MIN_WLS_SATELLITES) + { + return active_mask; + } + return eligible_mask; +} + +#if (GPS_EPHEMERIS_FLASH_ENABLE == 1) +static void gps_flash_read(void* ctx, uint32_t address, void* dst, + uint32_t len) +{ + (void)ctx; + bsp_spi_flash_read(address, dst, len); +} + +static bool gps_flash_erase(void* ctx, uint32_t address) +{ + (void)ctx; + return bsp_spi_flash_erase_sector(address); +} + +static bool gps_flash_program(void* ctx, uint32_t address, const void* src, + uint32_t len) +{ + (void)ctx; + return bsp_spi_flash_program(address, src, len); +} + +static void gps_ephemeris_flash_loaded(void* ctx, unsigned int prn, + const gps_ephemeris_t* ephemeris) +{ + (void)ctx; + /* + * Persisted records enter the RAM cache as candidates, marked as restored. + * They are not trusted for navigation yet: gps_ephemeris_cache_restore_at() + * re-checks them against the curve fit interval as soon as receiver time + * exists, and refuses them until then only if they fail the structural + * rules. The marking is what lets the first live broadcast for that PRN + * replace one outright, instead of losing a Toe comparison that cannot see + * across a week boundary. + */ + if(gps_ephemeris_cache_store_restored(&gps_ephemeris_cache, prn, ephemeris, + gps_get_systick())) + { + GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, + "flash ephemeris candidate: prn=%u iode=%u toe=%.0f\r\n", + prn, ephemeris->iode, ephemeris->toe.sec); + } +} + +/* + * Persistent navigation context: almanac, ionosphere model, last position. + * Kept separate from the ephemeris store because it has a different lifetime + * (weeks, not hours) and a different shape (one small blob, not a per-PRN log). + */ +static bool gps_receiver_time_now(int* week_out, double* tow_sec_out); + +static gps_nav_context_store_t gps_nav_context_store; +static bool gps_nav_context_ready = false; +static bool gps_nav_context_position_seed_valid = false; +static double gps_nav_context_position_seed[3]; +/* + * The position exactly as it stands in the persisted payload, together with + * the receiver time it was taken at. Held rather than resampled so that a + * stationary receiver offers byte-identical payloads and the store's CRC + * comparison keeps the flash untouched; see gps_nav_context_position_changed(). + */ +static uint32_t gps_nav_context_position_week; +static double gps_nav_context_position_tow_sec; +static uint64_t gps_nav_context_position_tick; + +/* + * Restore what the receiver knew before the last power cycle. + * + * Everything here is re-parsed through the live decoder rather than adopted as + * a struct, so a stale or corrupt record fails exactly as a bad broadcast + * would. Nothing restored here is trusted for navigation on its own: the + * almanac only biases which PRNs are searched, the ionosphere model is a + * correction that is replaced as soon as page 18 is heard again, and the + * position is only a seed for the visibility prediction until the first real + * fix replaces it. + */ +static void gps_nav_context_restore(void) +{ + gps_nav_context_payload_t payload; + if(!gps_nav_context_store_load(&gps_nav_context_store, &payload)) + { + GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, + "no persisted navigation context\r\n"); + return; + } + + unsigned int almanac_pages = 0u; + for(unsigned int prn_index = 0u; + prn_index < GPS_ALMANAC_PRN_COUNT; + prn_index++) + { + if((payload.almanac_present_mask & (1u << prn_index)) == 0u) + { + continue; + } + unsigned int words[10]; + for(unsigned int i = 0u; i < 10u; i++) + { + words[i] = (unsigned int)payload.almanac_words[prn_index][i]; + } + /* SV IDs 1..24 ride subframe 5, 25..32 subframe 4. */ + unsigned char subframe_id = (prn_index < 24u) ? 5u : 4u; + if(gps_lnav_restore_almanac_page(words, subframe_id)) + { + almanac_pages++; + } + } + + if(payload.almanac_reference_valid != 0u) + { + gps_lnav_restore_almanac_reference( + payload.almanac_week, (double)payload.almanac_toa_sec); + } + + bool iono_restored = false; + if(payload.iono_valid != 0u) + { + unsigned int words[10]; + for(unsigned int i = 0u; i < 10u; i++) + { + words[i] = (unsigned int)payload.iono_words[i]; + } + iono_restored = gps_lnav_restore_iono_page(words); + } + + if(payload.position_valid != 0u) + { + double radius = + sqrt((payload.position_ecef_m[0] * payload.position_ecef_m[0]) + + (payload.position_ecef_m[1] * payload.position_ecef_m[1]) + + (payload.position_ecef_m[2] * payload.position_ecef_m[2])); + /* Only a sanity check: the seed just has to be on the planet. */ + if(isfinite(radius) && (radius > 6.0e6) && (radius < 7.0e6)) + { + gps_nav_context_position_seed[0] = payload.position_ecef_m[0]; + gps_nav_context_position_seed[1] = payload.position_ecef_m[1]; + gps_nav_context_position_seed[2] = payload.position_ecef_m[2]; + gps_nav_context_position_seed_valid = true; + /* + * Carry the stored time stamp forward verbatim. Re-deriving it + * would change the payload bytes on the first save after every + * boot and cost an erase for information nothing read. + */ + gps_nav_context_position_week = payload.position_week; + gps_nav_context_position_tow_sec = payload.position_tow_sec; + } + } + + GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV, + "navigation context restored: almanac=%u iono=%u position=%u generation=%lu\r\n", + almanac_pages, iono_restored ? 1u : 0u, + gps_nav_context_position_seed_valid ? 1u : 0u, + (unsigned long)gps_nav_context_store.generation); +} + +static void gps_nav_context_flash_init(void) +{ + gps_ephemeris_store_flash_t flash; + memset(&flash, 0, sizeof(flash)); + flash.read = gps_flash_read; + flash.erase_sector = gps_flash_erase; + flash.program = gps_flash_program; + flash.ctx = NULL; + flash.base_address = (uint32_t)GPS_NAV_CONTEXT_FLASH_BASE; + flash.sector_size = BSP_SPI_FLASH_SECTOR_SIZE; + + gps_nav_context_ready = + gps_nav_context_store_mount(&gps_nav_context_store, &flash); + if(!gps_nav_context_ready) + { + GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, + "persistent navigation context unavailable\r\n"); + return; + } + gps_nav_context_restore(); +} + +/* + * Rebuild the context from live state and persist it if - and only if - the + * bytes changed. The store compares a CRC before touching the flash, so this + * running on a timer costs nothing until the constellation actually publishes + * something new. + */ +static void gps_nav_context_flash_service(void) +{ + static uint64_t last_save_tick = 0u; + if(!gps_nav_context_ready) + { + return; + } + uint64_t now = gps_get_systick(); + if((last_save_tick != 0u) && + ((now - last_save_tick) < GPS_NAV_CONTEXT_SAVE_PERIOD_TICKS)) + { + return; + } + last_save_tick = now; + + gps_nav_context_payload_t payload; + memset(&payload, 0, sizeof(payload)); + + payload.almanac_present_mask = + gps_lnav_get_almanac_pages(payload.almanac_words); + + const gps_almanac_t* almanac = gps_lnav_get_almanac(); + if(almanac->reference_valid) + { + payload.almanac_reference_valid = 1u; + payload.almanac_week = (uint32_t)almanac->week; + payload.almanac_toa_sec = (uint32_t)almanac->toa_sec; + } + + payload.iono_valid = + gps_lnav_get_iono_page(payload.iono_words) ? 1u : 0u; + + /* + * Refresh the held seed only when a cold start would actually be aimed + * differently - the receiver has moved kilometres - and no more often than + * the refresh period. Both limits exist to keep the payload bytes stable: + * the store writes whenever they change, and a raw WLS position changes + * every epoch, so without this the erase count follows the save timer + * instead of following real news. A moving platform still refreshes its + * seed, just at a cadence set by how far it has gone rather than by how + * long the receiver has been on. + */ + double ecef[3]; + if(gps_nav_last_receiver_ecef(ecef)) + { + bool moved = !gps_nav_context_position_seed_valid || + gps_nav_context_position_changed( + gps_nav_context_position_seed, ecef, + GPS_NAV_CONTEXT_POSITION_EPSILON_M); + bool due = (gps_nav_context_position_tick == 0u) || + ((now - gps_nav_context_position_tick) >= + GPS_NAV_CONTEXT_POSITION_REFRESH_TICKS); + if(moved && due) + { + int week = 0; + double tow_sec = 0.0; + gps_nav_context_position_seed[0] = ecef[0]; + gps_nav_context_position_seed[1] = ecef[1]; + gps_nav_context_position_seed[2] = ecef[2]; + gps_nav_context_position_seed_valid = true; + gps_nav_context_position_tick = now; + if(gps_receiver_time_now(&week, &tow_sec)) + { + gps_nav_context_position_week = (uint32_t)week; + gps_nav_context_position_tow_sec = tow_sec; + } + } + } + if(gps_nav_context_position_seed_valid) + { + payload.position_valid = 1u; + payload.position_ecef_m[0] = gps_nav_context_position_seed[0]; + payload.position_ecef_m[1] = gps_nav_context_position_seed[1]; + payload.position_ecef_m[2] = gps_nav_context_position_seed[2]; + payload.position_week = gps_nav_context_position_week; + payload.position_tow_sec = gps_nav_context_position_tow_sec; + } + + /* Nothing worth persisting yet. */ + if((payload.almanac_present_mask == 0u) && (payload.iono_valid == 0u) && + (payload.position_valid == 0u)) + { + return; + } + + bool wrote = false; + if(!gps_nav_context_store_save(&gps_nav_context_store, &payload, &wrote)) + { + gps_nav_context_ready = false; + GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, + "navigation context save failed; persistence disabled\r\n"); + return; + } + if(wrote) + { + GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV, + "navigation context saved: almanac=0x%08lx iono=%u position=%u generation=%lu writes=%lu erases=%lu skipped=%lu\r\n", + (unsigned long)payload.almanac_present_mask, + payload.iono_valid, payload.position_valid, + (unsigned long)gps_nav_context_store.generation, + (unsigned long)gps_nav_context_store.writes, + (unsigned long)gps_nav_context_store.erases, + (unsigned long)gps_nav_context_store.skipped_writes); + } +} + +static void gps_ephemeris_flash_init(void) +{ + gps_ephemeris_store_flash_t flash; + memset(&flash, 0, sizeof(flash)); + flash.read = gps_flash_read; + flash.erase_sector = gps_flash_erase; + flash.program = gps_flash_program; + flash.ctx = NULL; + flash.base_address = (uint32_t)GPS_EPHEMERIS_FLASH_BASE; + flash.sector_size = BSP_SPI_FLASH_SECTOR_SIZE; + + gps_ephemeris_flash_pending_mask = 0u; + gps_ephemeris_store_ready = + gps_ephemeris_store_mount(&gps_ephemeris_store, &flash); + if(!gps_ephemeris_store_ready) + { + GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, + "persistent ephemeris store unavailable\r\n"); + return; + } +#if (GPS_EPHEMERIS_STORE_MAX_SLOTS != 0u) + gps_ephemeris_store_limit_slots(&gps_ephemeris_store, + GPS_EPHEMERIS_STORE_MAX_SLOTS); +#endif + unsigned int loaded = gps_ephemeris_store_load_all( + &gps_ephemeris_store, gps_ephemeris_flash_loaded, NULL); + gps_runtime_diagnostics.flash_ephemeris_loaded_total = loaded; + + /* + * A blank or unreadable store needs two sector erases before it can hold + * anything. Spend them here, during init, rather than letting the first + * ephemeris commit trigger roughly a second of erase stall in the middle of + * cold-start subframe decoding. + */ + if((gps_ephemeris_store.generation == 0u) || + (GPS_EPHEMERIS_FLASH_FORCE_FORMAT != 0)) + { + if(gps_ephemeris_store_format(&gps_ephemeris_store)) + { + GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, + "persistent ephemeris store formatted\r\n"); + } + else + { + gps_ephemeris_store_ready = false; + gps_runtime_diagnostics.flash_ephemeris_error_total++; + GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, + "persistent ephemeris store format failed; persistence disabled\r\n"); + } + } + GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, + "persistent ephemeris store mounted: generation=%u loaded=%u rejected=%u\r\n", + gps_ephemeris_store.generation, loaded, + gps_ephemeris_store.records_rejected); +} + +/* Write at most one deferred record; called from the background task only. */ +static void gps_ephemeris_flash_service(void) +{ + if(!gps_ephemeris_store_ready || (gps_ephemeris_flash_pending_mask == 0u)) + { + return; + } + + for(unsigned int prn = 0u; prn < GPS_PRN_COUNT; prn++) + { + uint32_t bit = 1u << prn; + if((gps_ephemeris_flash_pending_mask & bit) == 0u) + { + continue; + } + gps_ephemeris_flash_pending_mask &= ~bit; + + gps_ephemeris_t ephemeris; + if(!gps_ephemeris_cache_restore(&gps_ephemeris_cache, prn, + gps_get_systick(), &ephemeris)) + { + return; + } + bool wrote = false; + if(gps_ephemeris_store_save( + &gps_ephemeris_store, prn, &ephemeris, + (const unsigned int (*)[10])gps_ephemeris_flash_words[prn], + &wrote)) + { + if(wrote) + { + gps_runtime_diagnostics.flash_ephemeris_write_total++; + GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, + "persisted ephemeris: prn=%u iode=%u writes=%u erases=%u\r\n", + prn, ephemeris.iode, + gps_ephemeris_store.writes, + gps_ephemeris_store.erases); + } + } + else + { + gps_runtime_diagnostics.flash_ephemeris_error_total++; + if(gps_ephemeris_store.capacity_exceeded) + { + /* The store cannot hold the live set in one sector. Stop + * using it rather than let it keep erasing. */ + gps_ephemeris_store_ready = false; + gps_ephemeris_flash_pending_mask = 0u; + GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_NAV, + "persistent ephemeris disabled: live set exceeds one sector\r\n"); + } + else + { + GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, + "persistent ephemeris write failed: prn=%u\r\n", prn); + } + } + return; /* one record per pass */ + } +} + +/* Queue a freshly committed ephemeris for persistence. */ +static void gps_ephemeris_flash_queue(unsigned int prn, + const unsigned int words[3][10]) +{ + if(!gps_ephemeris_store_ready || (prn >= GPS_PRN_COUNT)) + { + return; + } + for(unsigned int sf = 0u; sf < 3u; sf++) + { + for(unsigned int word = 0u; word < 10u; word++) + { + gps_ephemeris_flash_words[prn][sf][word] = words[sf][word]; + } + } + gps_ephemeris_flash_pending_mask |= (1u << prn); +} +#endif /* GPS_EPHEMERIS_FLASH_ENABLE */ + +static void gps_prn_context_store(unsigned int prn, + int week, + double tow_sec, + uint64_t pps_counter, + uint32_t raw_sample, + double corrected_sample, + uint64_t tick) +{ + if(!gps_prn_valid(prn) || (week < 0) || + !(tow_sec >= 0.0) || (tow_sec >= GPS_TIME_WEEK_SECONDS) || + (pps_counter == 0u)) + { + return; + } + gps_prn_nav_context_t* context = &gps_prn_context[prn]; + context->week = week; + context->tow_sec = tow_sec; + context->pps_counter = pps_counter; + context->raw_sample = raw_sample; + context->corrected_sample = corrected_sample; + context->tick = tick; + context->valid = true; +} + +/* + * Navigation-bit phase for a returning PRN, evaluated entirely in the PPS + * counter domain. 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 can span a multi-minute outage unambiguously. + * + * Returns false when the context is too old, when the receiver is too close to + * a navigation-bit boundary for the arm to be placed unambiguously, or when the + * arithmetic is not finite. Every rejection simply means the channel falls + * back to waiting for a decoded subframe. + */ +static bool gps_warm_seed_phase(unsigned int prn, uint32_t* phase_out) +{ + if(!gps_prn_valid(prn) || (phase_out == NULL)) + { + return false; + } + const gps_prn_nav_context_t* context = &gps_prn_context[prn]; + if(!context->valid) + { + return false; + } + + uint64_t now_counter = gps_pps_read_raw_counter(); + if(now_counter <= context->pps_counter) + { + return false; + } + double elapsed_sec = (double)(now_counter - context->pps_counter) / + (double)GPS_PPS_CLOCK_HZ; + if(!isfinite(elapsed_sec) || (elapsed_sec <= 0.0) || + (elapsed_sec > GPS_NAV_WARM_SEED_MAX_AGE_SEC)) + { + return false; + } + + return gps_nav_bit_phase_from_elapsed( + context->tow_sec, elapsed_sec, + (double)GPS_NAV_SOLVE_TIMER_TICK_MS / 1000.0, + GPS_NAV_SOLVE_TIMER_TICKS, + GPS_NAV_WARM_SEED_GUARD_SEC, + phase_out); +} + +static void gps_receiver_time_update(int week, double tow_sec, uint64_t tick) +{ + if((week < 0) || !(tow_sec >= 0.0) || + (tow_sec >= GPS_TIME_WEEK_SECONDS)) + { + return; + } + gps_receiver_time_week = week; + gps_receiver_time_tow_sec = tow_sec; + gps_receiver_time_tick = tick; + gps_receiver_time_valid = true; +} + +static bool gps_receiver_time_now(int* week_out, double* tow_sec_out) +{ + if(!gps_receiver_time_valid || + (week_out == NULL) || (tow_sec_out == NULL)) + { + return false; + } + + uint64_t elapsed_ticks = gps_get_systick() - gps_receiver_time_tick; + double tow_sec = gps_receiver_time_tow_sec + + ((double)elapsed_ticks / (double)GPS_SYSTICK_HZ); + int week = gps_receiver_time_week; + while(tow_sec >= GPS_TIME_WEEK_SECONDS) + { + tow_sec -= GPS_TIME_WEEK_SECONDS; + week++; + } + *week_out = week; + *tow_sec_out = tow_sec; + return true; } static uint64_t gps_systick_interval(void) @@ -328,6 +1024,87 @@ static unsigned int gps_take_plic_repeat_count(void) return count; } +/* + * Recompute which PRNs the almanac predicts are above the elevation mask and + * hand the result to the acquisition scheduler. + * + * Every input here is optional. Without an almanac, without a resolved + * receiver time, or before the first position fix, the mask stays all-ones and + * acquisition sweeps exactly as it did before. The prediction only ever + * changes how often a PRN is retried, never whether it can be retried. + */ +static void gps_refresh_acq_visibility(void) +{ + static uint64_t last_visibility_tick = 0u; + uint64_t now = gps_get_systick(); + if((last_visibility_tick != 0u) && + ((now - last_visibility_tick) < GPS_ACQ_VISIBILITY_REFRESH_TICKS)) + { + return; + } + last_visibility_tick = now; + + int week = 0; + double tow_sec = 0.0; + double receiver_ecef[3]; + if(!gps_receiver_time_now(&week, &tow_sec)) + { + gps_acq_manager_set_search_mask(&gps_acq_manager, 0xffffffffu); + return; + } + double mask_elevation_deg = GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG; + if(!gps_nav_last_receiver_ecef(receiver_ecef)) + { + /* + * Before the first fix, fall back to the position persisted from the + * last run. That is what makes a restored almanac useful at cold + * start instead of only after the receiver has already navigated. + * + * If the receiver has been moved a long way since, the prediction is + * wrong - but it is advisory, so the cost is that some PRNs are + * retried slowly, and it self-corrects the moment a real fix replaces + * the seed. + */ + if(!gps_nav_context_position_seed_valid) + { + gps_acq_manager_set_search_mask(&gps_acq_manager, 0xffffffffu); + return; + } + receiver_ecef[0] = gps_nav_context_position_seed[0]; + receiver_ecef[1] = gps_nav_context_position_seed[1]; + receiver_ecef[2] = gps_nav_context_position_seed[2]; + /* + * A seed read back from flash says where the receiver was, not where + * it is, and nothing has yet confirmed the two are the same place. + * Widen the mask while that is true, so a receiver carried a long way + * between power cycles still searches the satellites that have risen + * at the new site. A displacement of D km moves a look angle by + * roughly D/111 degrees, so this covers a couple of thousand + * kilometres of unknown migration and still excludes the far side of + * the earth - which is where three quarters of the wasted searches + * were. The first real fix replaces the seed and the mask closes + * back to its normal value. + */ + mask_elevation_deg = GPS_ACQ_VISIBILITY_SEED_MASK_ELEVATION_DEG; + } + + uint32_t mask = 0xffffffffu; + unsigned int visible = gps_almanac_visible_mask( + gps_lnav_get_almanac(), (unsigned int)week, tow_sec, + receiver_ecef, mask_elevation_deg, &mask); + + uint32_t previous = gps_acq_manager_search_mask(&gps_acq_manager); + gps_acq_manager_set_search_mask(&gps_acq_manager, mask); + if(mask != previous) + { + GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_ACQ, + "visibility updated: visible=%u almanac=%u mask=0x%08lx elev_mask=%.0f\r\n", + visible, + gps_almanac_valid_count(gps_lnav_get_almanac()), + (unsigned long)mask, mask_elevation_deg); + } +} + static bool gps_take_acq_schedule_pending(void) { uint64_t irq_state = bsp_irq_save(); @@ -362,6 +1139,7 @@ static void gps_nav_publish_active_snapshot(uint32_t channel_mask, if(gps_mask_count(channel_mask) < GPS_NAV_MIN_WLS_SATELLITES) { gps_runtime_diagnostics.nav_epoch_incomplete_total++; + gps_runtime_diagnostics.nav_epoch_below_minimum_total++; gps_nav_discard_active_snapshot(reset_assembly); return; } @@ -389,6 +1167,12 @@ bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot) now_local_ticks)) { gps_runtime_diagnostics.nav_epoch_incomplete_total++; + gps_runtime_diagnostics.nav_epoch_assembly_failed_total++; + gps_nav_epoch_account(gps_nav_epoch_assembly.expected_mask, + gps_nav_epoch_assembly.received_mask, + gps_nav_epoch_assembly.minimum_capture_counter, + gps_nav_epoch_assembly.maximum_capture_counter, + GPS_EPOCH_ACCT_DEADLINE); /* An epoch is an all-or-nothing hardware measurement set. Publishing * a timed-out subset changes its satellite geometry according to IRQ * timing and was the source of large raw-position jumps. */ @@ -405,6 +1189,13 @@ bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot) return ready; } +void gps_report_ephemeris_rejection(unsigned int prn, unsigned int reason) +{ + gps_runtime_diagnostics.ephemeris_rejected_total++; + gps_runtime_diagnostics.last_ephemeris_reject_prn = prn; + gps_runtime_diagnostics.last_ephemeris_reject_reason = reason; +} + void gps_get_runtime_diagnostics(gps_runtime_diagnostics_t* diagnostics) { if(diagnostics == NULL) @@ -445,6 +1236,25 @@ static void gps_set_tracking_nav_bits_mode(unsigned char trck_ip_no) trck_enable_measurement_intr(trck_ips[trck_ip_no]); } +/* + * Same ordering as the ephemeris-driven path in gps_ip.c: nav-bit collection + * stays enabled so background subframe decoding continues, and the interrupt is + * unmasked last so hardware cannot raise a nav-solve IRQ before its source flag + * is observable. + */ +static void gps_enable_tracking_nav_solve_mode(unsigned char trck_ip_no) +{ + if(trck_ip_no >= GPS_TRCK_IP_COUNT) + { + return; + } + trck_enable_nav_bits_intr(trck_ips[trck_ip_no]); + trck_enable_nav_bits_flag(trck_ips[trck_ip_no]); + trck_enable_nav_solve(trck_ips[trck_ip_no]); + trck_enable_nav_solve_flag(trck_ips[trck_ip_no]); + trck_enable_nav_solve_intr(trck_ips[trck_ip_no]); +} + static void gps_runtime_state_init(void) { const gps_acq_manager_config_t acq_config = { @@ -452,7 +1262,10 @@ static void gps_runtime_state_init(void) .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 + .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_event_head = 0; @@ -470,12 +1283,22 @@ static void gps_runtime_state_init(void) gps_runtime_diagnostics.last_connected_prn = GPS_PRN_INVALID; gps_runtime_diagnostics.last_lost_prn = GPS_PRN_INVALID; gps_last_acq_status_log_tick = 0u; + gps_receiver_time_valid = false; + gps_receiver_time_week = 0; + gps_receiver_time_tow_sec = 0.0; + gps_receiver_time_tick = 0u; gps_external_irq_id = 0u; gps_external_irq_handler = NULL; gps_acq_manager_init(&gps_acq_manager, &acq_config); gps_ephemeris_cache_init( &gps_ephemeris_cache, GPS_EPHEMERIS_CACHE_MAX_AGE_TICKS); +#if (GPS_EPHEMERIS_FLASH_ENABLE == 1) + gps_ephemeris_flash_init(); +#if (GPS_NAV_CONTEXT_ENABLE == 1) + gps_nav_context_flash_init(); +#endif +#endif for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++) { @@ -493,12 +1316,17 @@ static void gps_runtime_state_init(void) { satellite_tracking_ip[i] = GPS_PRN_INVALID; memset(&satellite_acq_result[i], 0, sizeof(satellite_acq_result[i])); + memset(&gps_prn_context[i], 0, sizeof(gps_prn_context[i])); } for(unsigned int i = 0 ; i < GPS_TRCK_IP_COUNT ; i++) { tracking_allocated[i] = false; tracking_not_busy_polls[i] = 0u; + trk_seed_requested[i] = false; + trk_seed_unconfirmed[i] = false; + trk_seed_prn[i] = GPS_PRN_INVALID; + trk_nav_solve_miss[i] = 0u; memset(&trk_shadow[i], 0, sizeof(trk_shadow[i])); gps_clear_tracking_slot((unsigned char)i); } @@ -799,18 +1627,41 @@ bool assign_tracking_from_result(const gps_acq_result_t* result) tracking_not_busy_polls[i] = 0u; gps_clear_tracking_slot(i); gps_ephemeris_t cached_ephemeris; - if(gps_ephemeris_cache_restore( + int receiver_week = 0; + double receiver_tow_sec = 0.0; + bool time_known = gps_receiver_time_now( + &receiver_week, &receiver_tow_sec); + if(gps_ephemeris_cache_restore_at( &gps_ephemeris_cache, prn_no, - gps_get_systick(), &cached_ephemeris)) + gps_get_systick(), time_known, + receiver_week, receiver_tow_sec, + &cached_ephemeris)) { gps_frames_parsed[i].gps_ephems = cached_ephemeris; gps_runtime_diagnostics .ephemeris_cache_restore_total++; GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, - "restored ephemeris cache: prn=%u trk=%u iode=%u\r\n", - prn_no, i, cached_ephemeris.iode); + "restored ephemeris cache: prn=%u trk=%u iode=%u toe=%.0f time_known=%u\r\n", + prn_no, i, cached_ephemeris.iode, + cached_ephemeris.toe.sec, + (unsigned int)time_known); } + else + { + gps_runtime_diagnostics + .ephemeris_cache_miss_total++; + } + /* A returning PRN with usable ephemeris and a live PRN context can + * skip the subframe wait. The arm itself happens in the foreground + * once the channel reports lock, because it must be placed at a + * known point inside a navigation bit. */ + trk_seed_requested[i] = + gps_frames_parsed[i].gps_ephems.ephemeris_valid && + gps_prn_context[prn_no].valid; + trk_seed_unconfirmed[i] = false; + trk_seed_prn[i] = prn_no; + trk_assign_mtime[i] = bsp_read64(BSP_CLINT_MTIME); gps_set_tracking_nav_bits_mode(i); trck_ips[i]->PRN_NUMBER = result->prn; trck_ips[i]->CODE_PHASE = result->code_phase; @@ -1014,6 +1865,19 @@ static void gps_process_acq_result(const gps_acq_result_t* result) { gps_acq_manager_mark_failed( &gps_acq_manager, prn, result->tick); + /* + * result->peak_metric is deliberately not logged here: it is always 0 + * on a failed search. acqusition_wrapper.sv latches the metric only + * when the search succeeded + * peak_metric_reg_n = acq_stat_p ? peak_metric_syned : peak_metric_reg_p; + * and zeroes the register on every start_acq, so a failure leaves the + * zero behind. That makes "no satellite there" (peak/second-peak near + * 1.0) indistinguishable from "satellite present but under the fixed + * PEAK_THRESHOLD of 512/256 = 2.00 ratio" - which is exactly the + * measurement needed to tell a sky limit from a sensitivity limit on + * the tracked satellite count. Latching it unconditionally is a + * one-line RTL change on an already-existing capture path. + */ GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_ACQ, "satellite search ended: engine=%u prn=%u status=0x%x result=not_acquired\r\n", result->engine_id, prn, result->status); return; @@ -1151,6 +2015,7 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event) if(gps_prn_valid(dropped_prn)) { + gps_prn_drop_mtime[dropped_prn] = bsp_read64(BSP_CLINT_MTIME); gps_mark_satellite_lost(dropped_prn); } @@ -1162,6 +2027,12 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event) tracking_allocated[trck_ip_no] = false; tracking_not_busy_polls[trck_ip_no] = 0u; + /* Channel-scoped warm-start state dies with the channel. The PRN context + * it was derived from deliberately survives. */ + trk_seed_requested[trck_ip_no] = false; + trk_seed_unconfirmed[trck_ip_no] = false; + trk_seed_prn[trck_ip_no] = GPS_PRN_INVALID; + trk_nav_solve_miss[trck_ip_no] = 0u; if(trck_ip_exists(&tracking_node, trck_ip_no) && !delete_by_ip_number(&tracking_node, trck_ip_no)) { @@ -1243,22 +2114,64 @@ static void gps_process_tracking_nav_bits(const gps_isr_event_t* event) gps_frame_parsed->gps_ephems.abs_sample = event->corrected_abs_sample; gps_frame_parsed->gps_ephems.SVID = (unsigned char)event->prn; + /* + * Cross-check the broadcast issue of data against the ephemeris currently + * in use, before parse_subframe() can commit a replacement. A mismatch + * means the in-use record - typically one restored from RAM or flash - has + * been superseded by a newer upload. + * + * Navigation behaviour deliberately does not change here: IS-GPS-200 allows + * an older ephemeris to be used while it is inside its curve fit interval, + * and the fit window is already enforced per epoch. What this buys is + * knowing, within one subframe instead of a full set, that a refresh is + * coming. + */ + { + unsigned char in_use_iode = gps_frame_parsed->gps_ephems.iode; + unsigned char broadcast_iode = 0u; + unsigned char peek_id = + (unsigned char)((gps_subframe[1] >> 8) & 0x07u); + if(gps_frame_parsed->gps_ephems.ephemeris_valid && + gps_lnav_broadcast_iode((const unsigned int*)gps_subframe, + peek_id, &broadcast_iode) && + (broadcast_iode != in_use_iode)) + { + gps_runtime_diagnostics.ephemeris_iod_mismatch_total++; + GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, + "ephemeris superseded: prn=%u in_use_iode=%u broadcast_iode=%u sf=%u\r\n", + event->prn, in_use_iode, broadcast_iode, peek_id); + } + } + unsigned int how_tow_sec = 0u; unsigned char subframe_id = parse_subframe((unsigned int*)gps_subframe, (gps_frame_t*)gps_frame_parsed, &how_tow_sec); if((subframe_id >= 1u) && (subframe_id <= 3u) && - (gps_frame_parsed->pending_ephemeris_mask == 0u) && - gps_ephemeris_cache_store( - &gps_ephemeris_cache, event->prn, - (const gps_ephemeris_t*)&gps_frame_parsed->gps_ephems, - event->tick)) + (gps_frame_parsed->pending_ephemeris_mask == 0u)) { - gps_runtime_diagnostics.ephemeris_cache_store_total++; - GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, - "stored ephemeris cache: prn=%u trk=%u iode=%u\r\n", - event->prn, trck_ip_no, - gps_frame_parsed->gps_ephems.iode); + bool ephemeris_changed = false; + if(gps_ephemeris_cache_store_if_new( + &gps_ephemeris_cache, event->prn, + (const gps_ephemeris_t*)&gps_frame_parsed->gps_ephems, + event->tick, &ephemeris_changed) && + ephemeris_changed) + { + gps_runtime_diagnostics.ephemeris_cache_store_total++; +#if (GPS_EPHEMERIS_FLASH_ENABLE == 1) + /* Only a genuinely new data set reaches flash; the same + * broadcast decoded again is filtered out above. */ + gps_ephemeris_flash_queue( + event->prn, + (const unsigned int (*)[10]) + gps_frame_parsed->pending_ephemeris_words); +#endif + GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, + "stored ephemeris cache: prn=%u trk=%u iode=%u toe=%.0f\r\n", + event->prn, trck_ip_no, + gps_frame_parsed->gps_ephems.iode, + gps_frame_parsed->gps_ephems.toe.sec); + } } if((subframe_id >= 1u) && (subframe_id <= 5u) && (how_tow_sec < (unsigned int)GPS_TIME_WEEK_SECONDS)) @@ -1351,6 +2264,101 @@ static void gps_process_tracking_nav_bits(const gps_isr_event_t* event) trck_ip_no, event->prn, node->gps_subframe_mask, event->status); } +/* + * Turn the first epoch of a warm-seeded channel into a normal HOW/sample time + * anchor. The prediction only has to be good enough to identify which point of + * the nav-solve grid this epoch is; the snap then supplies the exact transmit + * time, and the ABS_SAMPLE latched by the same hardware pulse completes the + * anchor so every later epoch uses the ordinary sample-anchor path. + * + * The anchor is committed with gps_how_week_valid clear: the week still comes + * from the PRN context rather than a parity-checked subframe 1, and the first + * decoded subframe upgrades it. + */ +/* + * Latency from the events that bound the warm path. Uses CLINT mtime, not the + * 10 Hz systick, because the interesting quantity is one 200 ms epoch. + */ +static void gps_warm_seed_record_latency(unsigned char trck_ip_no, + unsigned int prn) +{ + uint64_t now = bsp_read64(BSP_CLINT_MTIME); + const uint64_t hz = (uint64_t)BSP_CLINT_TIMEBASE_HZ; + if((trck_ip_no < GPS_TRCK_IP_COUNT) && + (trk_assign_mtime[trck_ip_no] != 0u) && + (now > trk_assign_mtime[trck_ip_no])) + { + uint64_t us = ((now - trk_assign_mtime[trck_ip_no]) * 1000000u) / hz; + uint32_t value = (us > 0xffffffffu) ? 0xffffffffu : (uint32_t)us; + gps_runtime_diagnostics.warm_seed_lock_to_epoch_us = value; + if((gps_runtime_diagnostics.warm_seed_lock_to_epoch_min_us == 0u) || + (value < gps_runtime_diagnostics.warm_seed_lock_to_epoch_min_us)) + { + gps_runtime_diagnostics.warm_seed_lock_to_epoch_min_us = value; + } + if(value > gps_runtime_diagnostics.warm_seed_lock_to_epoch_max_us) + { + gps_runtime_diagnostics.warm_seed_lock_to_epoch_max_us = value; + } + } + if(gps_prn_valid(prn) && (gps_prn_drop_mtime[prn] != 0u) && + (now > gps_prn_drop_mtime[prn])) + { + uint64_t us = ((now - gps_prn_drop_mtime[prn]) * 1000000u) / hz; + gps_runtime_diagnostics.warm_seed_drop_to_epoch_us = + (us > 0xffffffffu) ? 0xffffffffu : (uint32_t)us; + } +} + +static bool gps_warm_seed_establish_anchor(volatile gps_ephem_ll_t* node, + const gps_isr_event_t* event) +{ + unsigned int prn = event->prn; + if((node == NULL) || !gps_prn_valid(prn) || !gps_prn_context[prn].valid) + { + return false; + } + const gps_prn_nav_context_t* context = &gps_prn_context[prn]; + if(event->pps_counter <= context->pps_counter) + { + return false; + } + + double elapsed_sec = (double)(event->pps_counter - context->pps_counter) / + (double)GPS_PPS_CLOCK_HZ; + if(!isfinite(elapsed_sec) || (elapsed_sec <= 0.0) || + (elapsed_sec > GPS_NAV_WARM_SEED_MAX_AGE_SEC)) + { + return false; + } + + double snapped_tow_sec = 0.0; + if(!gps_nav_epoch_snap_from_elapsed( + context->tow_sec, elapsed_sec, + (double)GPS_NAV_SOLVE_PERIOD_MS / 1000.0, + GPS_NAV_WARM_SEED_SNAP_SEC, &snapped_tow_sec)) + { + return false; + } + + int week = context->week; + while(snapped_tow_sec >= GPS_TIME_WEEK_SECONDS) + { + snapped_tow_sec -= GPS_TIME_WEEK_SECONDS; + week++; + } + + node->gps_how_tow.week = week; + node->gps_how_tow.sec = snapped_tow_sec; + node->gps_how_abs_sample = event->abs_sample_raw; + node->gps_how_corrected_abs_sample = event->corrected_abs_sample; + node->gps_how_tick = event->tick; + node->gps_how_anchor_valid = true; + node->gps_how_week_valid = false; + node->gps_pending_how_valid = false; + return true; +} + static void gps_process_tracking_nav_solve(const gps_isr_event_t* event) { unsigned char trck_ip_no = event->index; @@ -1407,6 +2415,36 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event) event->pps_capture_sequence; } + if(!node->gps_how_anchor_valid && trk_seed_unconfirmed[trck_ip_no] && + event->pps_capture_valid) + { + /* First epoch produced by a warm seed. No subframe has been + * decoded on this channel, so the transmit time is predicted from + * the PRN context in the PPS domain and then snapped to the known + * nav-solve grid. The snap is the proof: the hardware can only + * raise this epoch on a multiple of the nav-solve period, so a + * prediction that does not land there means the context no longer + * describes this satellite. */ + if(!gps_warm_seed_establish_anchor(node, event)) + { + gps_runtime_diagnostics.warm_seed_rejected_total++; + gps_runtime_diagnostics.warm_seed_snap_failed_total++; + trk_seed_unconfirmed[trck_ip_no] = false; + gps_set_tracking_nav_bits_mode(trck_ip_no); + GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, + "warm seed epoch rejected, reverting to subframe wait: trk=%u prn=%u\r\n", + trck_ip_no, event->prn); + return; + } + trk_seed_unconfirmed[trck_ip_no] = false; + gps_runtime_diagnostics.warm_seed_confirmed_total++; + gps_warm_seed_record_latency(trck_ip_no, event->prn); + GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, + "warm seed epoch accepted: trk=%u prn=%u week=%d tow=%.3f\r\n", + trck_ip_no, event->prn, + node->gps_how_tow.week, node->gps_how_tow.sec); + } + if(!node->gps_how_anchor_valid) { GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, @@ -1441,6 +2479,21 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event) gps_frame_parsed->gps_ephems.tow.week = tow_week; gps_frame_parsed->gps_ephems.tow.sec = tow_sec; gps_frame_parsed->gps_ephems.abs_sample = event->corrected_abs_sample; + /* Satellite transmit time is within ~80 ms of receiver time; that is + * far inside the resolution this coarse datum needs. */ + gps_receiver_time_update(tow_week, tow_sec, event->tick); + if(event->pps_capture_valid) + { + /* Transmit time, PPS counter and RF sample latch all come from the + * one hardware pulse, so this triple is internally coherent. It is + * kept per PRN, not per channel, so a later reacquisition on any + * channel can use it. */ + gps_prn_context_store(event->prn, tow_week, tow_sec, + event->pps_counter, + event->abs_sample_raw, + event->corrected_abs_sample, + event->tick); + } /* A one-entry hardware mailbox carries only the newest measurement. * Missed capture pulses are diagnostic counts, not synthetic samples. */ node->gps_nav_solve_sequence++; @@ -1459,8 +2512,22 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event) return; } + /* + * This event is proof the channel can emit, so clear its miss count + * here rather than relying on the epoch result. An excluded channel is + * absent from ready_mask, which makes gps_nav_epoch_assembly_push() + * reject its events as INVALID; it would then never appear in + * received_mask and never be re-admitted. Clearing on the event itself + * is what keeps the exclusion reversible. + */ + trk_nav_solve_miss[trck_ip_no] = 0u; + uint32_t previous_expected = gps_nav_epoch_assembly.expected_mask; uint32_t previous_received = gps_nav_epoch_assembly.received_mask; + uint64_t previous_minimum = + gps_nav_epoch_assembly.minimum_capture_counter; + uint64_t previous_maximum = + gps_nav_epoch_assembly.maximum_capture_counter; gps_nav_epoch_event_result_t assembly_result = gps_nav_epoch_assembly_push( &gps_nav_epoch_assembly, @@ -1475,6 +2542,10 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event) if(assembly_result == GPS_NAV_EPOCH_EVENT_RESTARTED) { gps_runtime_diagnostics.nav_epoch_incomplete_total++; + gps_runtime_diagnostics.nav_epoch_assembly_failed_total++; + gps_nav_epoch_account(previous_expected, previous_received, + previous_minimum, previous_maximum, + GPS_EPOCH_ACCT_RESTART); /* The first interrupt from a newer period proves the old expected * set will never complete. Drop it rather than solving a partial * satellite set. */ @@ -1514,6 +2585,11 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event) if(assembly_result == GPS_NAV_EPOCH_EVENT_COMPLETE) { + gps_nav_epoch_account(gps_nav_epoch_assembly.expected_mask, + gps_nav_epoch_assembly.received_mask, + gps_nav_epoch_assembly.minimum_capture_counter, + gps_nav_epoch_assembly.maximum_capture_counter, + GPS_EPOCH_ACCT_COMPLETE); gps_nav_publish_active_snapshot( gps_nav_epoch_assembly.received_mask, true); } @@ -1837,14 +2913,28 @@ static void gps_log_acq_status(void) unsigned int acquired = gps_acq_manager_count_state( &gps_acq_manager, GPS_ACQ_PRN_ACQUIRED); + const gps_almanac_t* almanac = gps_lnav_get_almanac(); + uint32_t search_mask = gps_acq_manager_search_mask(&gps_acq_manager); + unsigned int predicted_visible = 0u; + for(unsigned int bit = 0u; bit < GPS_ACQ_MANAGER_PRN_COUNT; bit++) + { + if((search_mask & (1u << bit)) != 0u) + { + predicted_visible++; + } + } + GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_ACQ, - "state=%s tracked=%u stale=%u hot=%u cold=%u acquiring=%u acquired=%u engine=%u prn=%u age_ms=%lu\r\n", + "state=%s tracked=%u stale=%u hot=%u cold=%u acquiring=%u acquired=%u engine=%u prn=%u age_ms=%lu alm=%u/%u searchable=%u\r\n", (tracking >= GPS_NAV_MIN_WLS_SATELLITES) ? "tracking" : "searching", tracking, stale, hot, cold, acquiring, acquired, engine, engine_prn, (unsigned long)((engine_age_ticks * 1000u) / - GPS_SYSTICK_HZ)); + GPS_SYSTICK_HZ), + gps_almanac_valid_count(almanac), + almanac->reference_valid ? 1u : 0u, + predicted_visible); } static void gps_mark_stale_measurements(void) @@ -1937,21 +3027,124 @@ static void gps_reconcile_stopped_tracking_channels(void) } } +/* + * Arm the tracking IP's epoch phase for channels that are reacquiring a PRN we + * already have timing for. Runs in the foreground, not the ISR: the phase + * computation is floating point, and the guard band inside + * gps_warm_seed_phase() rejects any attempt placed too near a navigation-bit + * boundary, so a late foreground pass simply retries on the next one. + */ +static void gps_service_warm_seed_requests(void) +{ +#if (GPS_WARM_SEED_ENABLE == 0) + /* Built for a bitstream without NAV_SOLVE_PHASE_SEED: never touch the + * register, and let every channel take the subframe path. */ + for(unsigned char i = 0u ; i < GPS_TRCK_IP_COUNT ; i++) + { + trk_seed_requested[i] = false; + trk_seed_unconfirmed[i] = false; + } +#else + for(unsigned char i = 0u ; i < GPS_TRCK_IP_COUNT ; i++) + { + if(!trk_seed_requested[i]) + { + continue; + } + + uint32_t status = trck_ips[i]->STATUS_REG; + bool busy = (status & (1u << GPS_TRCK_BUSY_POS)) != 0u; + bool locked = (status & (1u << GPS_TRCK_LOCK_POS)) != 0u; + if(!tracking_allocated[i] || !busy || !locked) + { + /* Not tracking yet, or the channel went away before we armed. */ + if(!tracking_allocated[i]) + { + trk_seed_requested[i] = false; + } + continue; + } + unsigned int prn = trk_seed_prn[i]; + if(!gps_prn_valid(prn) || (trck_ips[i]->PRN_NUMBER != prn)) + { + /* The channel is no longer the PRN this request was raised for. */ + trk_seed_requested[i] = false; + continue; + } + + uint32_t phase = 0u; + if(!gps_warm_seed_phase(prn, &phase)) + { + /* Guard band or a stale context: retry, or give up once the + * context can no longer be used at all. */ + if(!gps_prn_context[prn].valid) + { + trk_seed_requested[i] = false; + } + continue; + } + + /* The node must exist before the first seeded epoch arrives, because + * the nav-solve path attaches its time anchor to it. */ + if(!trck_ip_exists(&tracking_node, i)) + { + (void)insert_element(&tracking_node, + &(gps_frames_parsed[i].gps_ephems), i); + } + volatile gps_ephem_ll_t* node = + get_element_by_ip_number(&tracking_node, i); + if(node == NULL) + { + trk_seed_requested[i] = false; + continue; + } + + trck_arm_nav_solve_phase_seed(trck_ips[i], phase); + trk_seed_requested[i] = false; + if(trck_nav_solve_provisional(trck_ips[i])) + { + trk_seed_unconfirmed[i] = true; + gps_enable_tracking_nav_solve_mode(i); + gps_runtime_diagnostics.warm_seed_armed_total++; + GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, + "warm seed armed: trk=%u prn=%u phase=%u\r\n", + i, prn, phase); + } + else + { + gps_runtime_diagnostics.warm_seed_rejected_total++; + gps_runtime_diagnostics.warm_seed_refused_total++; + GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, + "warm seed refused by tracking IP: trk=%u prn=%u phase=%u\r\n", + i, prn, phase); + } + } +#endif +} + void gps_background_task(void) { gps_poll_acq_pending_completions(); gps_poll_tracking_pending_events(); gps_process_isr_events(); + gps_service_warm_seed_requests(); if(gps_take_timer_maintenance_pending()) { gps_pps_service(); gps_reconcile_stopped_tracking_channels(); gps_mark_stale_measurements(); gps_check_acq_stuck_busy(); + gps_refresh_acq_visibility(); gps_log_acq_status(); } if(gps_take_acq_schedule_pending()) { gps_acq_scheduler_run(); } +#if (GPS_EPHEMERIS_FLASH_ENABLE == 1) + gps_ephemeris_flash_service(); +#endif +#if (GPS_NAV_CONTEXT_ENABLE == 1) + gps_nav_context_flash_service(); +#endif } diff --git a/soc/software/gps_system/src/gps/gps_ip.c b/soc/software/gps_system/src/gps/gps_ip.c index 9c44734..a4b6cb7 100644 --- a/soc/software/gps_system/src/gps/gps_ip.c +++ b/soc/software/gps_system/src/gps/gps_ip.c @@ -5,6 +5,7 @@ #include #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++) { diff --git a/soc/software/gps_system/src/gps/gps_lnav_parser.c b/soc/software/gps_system/src/gps/gps_lnav_parser.c new file mode 100644 index 0000000..77052ac --- /dev/null +++ b/soc/software/gps_system/src/gps/gps_lnav_parser.c @@ -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 +#include +#include +#include + +#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; +} diff --git a/soc/software/gps_system/src/gps/gps_nav_context_store.c b/soc/software/gps_system/src/gps/gps_nav_context_store.c new file mode 100644 index 0000000..3844a93 --- /dev/null +++ b/soc/software/gps_system/src/gps/gps_nav_context_store.c @@ -0,0 +1,256 @@ +#include "gps/gps_nav_context_store.h" + +#include +#include +#include + +#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; +} diff --git a/soc/software/gps_system/src/gps/gps_nav_epoch.c b/soc/software/gps_system/src/gps/gps_nav_epoch.c index 0574daf..68f3a15 100644 --- a/soc/software/gps_system/src/gps/gps_nav_epoch.c +++ b/soc/software/gps_system/src/gps/gps_nav_epoch.c @@ -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; diff --git a/soc/software/gps_system/src/gps/gps_pps.c b/soc/software/gps_system/src/gps/gps_pps.c index c64fbb7..29ad215 100644 --- a/soc/software/gps_system/src/gps/gps_pps.c +++ b/soc/software/gps_system/src/gps/gps_pps.c @@ -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) { diff --git a/soc/software/gps_system/src/gps/gps_pps_time_filter.c b/soc/software/gps_system/src/gps/gps_pps_time_filter.c index e1ead4f..8181d7c 100644 --- a/soc/software/gps_system/src/gps/gps_pps_time_filter.c +++ b/soc/software/gps_system/src/gps/gps_pps_time_filter.c @@ -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; } diff --git a/soc/software/gps_system/src/gps/gps_time_anchor.c b/soc/software/gps_system/src/gps/gps_time_anchor.c index e52a8b9..1918f08 100644 --- a/soc/software/gps_system/src/gps/gps_time_anchor.c +++ b/soc/software/gps_system/src/gps/gps_time_anchor.c @@ -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, diff --git a/soc/software/gps_system/tests/Makefile b/soc/software/gps_system/tests/Makefile index dd1c62d..e5d3f36 100644 --- a/soc/software/gps_system/tests/Makefile +++ b/soc/software/gps_system/tests/Makefile @@ -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 \ diff --git a/soc/software/gps_system/tests/test_gps_acq_manager.c b/soc/software/gps_system/tests/test_gps_acq_manager.c index caf2aa6..312ae0b 100644 --- a/soc/software/gps_system/tests/test_gps_acq_manager.c +++ b/soc/software/gps_system/tests/test_gps_acq_manager.c @@ -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(); diff --git a/soc/software/gps_system/tests/test_gps_almanac.c b/soc/software/gps_system/tests/test_gps_almanac.c new file mode 100644 index 0000000..b49bde0 --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_almanac.c @@ -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 +#include +#include +#include + +#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; +} diff --git a/soc/software/gps_system/tests/test_gps_ephemeris_cache.c b/soc/software/gps_system/tests/test_gps_ephemeris_cache.c index 3d3bba6..3881fcc 100644 --- a/soc/software/gps_system/tests/test_gps_ephemeris_cache.c +++ b/soc/software/gps_system/tests/test_gps_ephemeris_cache.c @@ -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; } diff --git a/soc/software/gps_system/tests/test_gps_ephemeris_store.c b/soc/software/gps_system/tests/test_gps_ephemeris_store.c new file mode 100644 index 0000000..c13c10b --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_ephemeris_store.c @@ -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 +#include +#include +#include +#include + +#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; +} diff --git a/soc/software/gps_system/tests/test_gps_ephemeris_validity.c b/soc/software/gps_system/tests/test_gps_ephemeris_validity.c new file mode 100644 index 0000000..e68b1e6 --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_ephemeris_validity.c @@ -0,0 +1,253 @@ +#include +#include +#include +#include + +#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, ¤t)); + + /* 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, ¤t)); + + /* 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, ¤t)); + + /* 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, ¤t)); + + /* 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, ¤t)); + assert(gps_ephemeris_supersedes(¤t, NULL)); + assert(!gps_ephemeris_supersedes(NULL, ¤t)); +} + +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; +} diff --git a/soc/software/gps_system/tests/test_gps_lnav_iode.c b/soc/software/gps_system/tests/test_gps_lnav_iode.c new file mode 100644 index 0000000..3b76dcc --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_lnav_iode.c @@ -0,0 +1,168 @@ +#include +#include +#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; +} diff --git a/soc/software/gps_system/tests/test_gps_measurement_model.c b/soc/software/gps_system/tests/test_gps_measurement_model.c new file mode 100644 index 0000000..29f0834 --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_measurement_model.c @@ -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 +#include +#include +#include + +#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; +} diff --git a/soc/software/gps_system/tests/test_gps_nav_context_audit.c b/soc/software/gps_system/tests/test_gps_nav_context_audit.c new file mode 100644 index 0000000..6556995 --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_nav_context_audit.c @@ -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 +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/soc/software/gps_system/tests/test_gps_nav_context_store.c b/soc/software/gps_system/tests/test_gps_nav_context_store.c new file mode 100644 index 0000000..b1b0c46 --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_nav_context_store.c @@ -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 +#include +#include +#include +#include + +#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; +} diff --git a/soc/software/gps_system/tests/test_gps_pvt_consistency.c b/soc/software/gps_system/tests/test_gps_pvt_consistency.c new file mode 100644 index 0000000..cfccfaf --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_pvt_consistency.c @@ -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 +#include +#include +#include +#include +#include + +#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; +} diff --git a/soc/software/gps_system/tests/test_gps_time_anchor.c b/soc/software/gps_system/tests/test_gps_time_anchor.c index 826d045..0644a7a 100644 --- a/soc/software/gps_system/tests/test_gps_time_anchor.c +++ b/soc/software/gps_system/tests/test_gps_time_anchor.c @@ -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(); diff --git a/soc/software/gps_system/tests/test_gps_warm_reacquisition_sim.c b/soc/software/gps_system/tests/test_gps_warm_reacquisition_sim.c new file mode 100644 index 0000000..a511536 --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_warm_reacquisition_sim.c @@ -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 +#include +#include +#include +#include + +#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; +}