diff --git a/hardware/external/gps/rtl/apb_wrappers/tracking_wrapper.sv b/hardware/external/gps/rtl/apb_wrappers/tracking_wrapper.sv index 60249ec..49c5b31 100644 --- a/hardware/external/gps/rtl/apb_wrappers/tracking_wrapper.sv +++ b/hardware/external/gps/rtl/apb_wrappers/tracking_wrapper.sv @@ -40,6 +40,14 @@ module tracking_wrapper #( import gps_top_pkg::*; import trck_pkg::*; + localparam int unsigned MEAS_CARRIER_WL = + ACQUIRED_FREQ_IL + TRCK_CARR_NCO_FREQ_FL; + localparam int unsigned MEAS_SUM_WL = + TRCK_CORR_WL + $clog2(MS_PER_BIT); + localparam int unsigned MEAS_WBP_WL = + (2 * TRCK_CORR_WL) + 1 + $clog2(MS_PER_BIT); + localparam int unsigned MEAS_COUNT_WL = $clog2(MS_PER_BIT + 1); + // apb state machine apb_state_e apb_bus_ps, apb_bus_ns; // tracking lock state machine @@ -70,6 +78,22 @@ module tracking_wrapper #( logic nav_bit, nav_bit_syned; // navigation bits. logic [31 : 0] abs_sample, abs_sample_syned; logic [REM_CODE_PH_I_WL-1 : 0] rem_code_ph, rem_code_ph_syned; + logic measurement_valid, measurement_valid_syned; + logic measurement_bit, measurement_bit_syned; + logic [31:0] measurement_abs_sample, measurement_abs_sample_syned; + logic [REM_CODE_PH_I_WL-1:0] measurement_rem_code_ph; + logic [REM_CODE_PH_I_WL-1:0] measurement_rem_code_ph_syned; + logic signed [MEAS_CARRIER_WL-1:0] measurement_carrier_freq_q7; + logic [MEAS_CARRIER_WL-1:0] measurement_carrier_freq_q7_syned; + logic signed [MEAS_SUM_WL-1:0] measurement_cn0_sum_i; + logic signed [MEAS_SUM_WL-1:0] measurement_cn0_sum_q; + logic [MEAS_SUM_WL-1:0] measurement_cn0_sum_i_syned; + logic [MEAS_SUM_WL-1:0] measurement_cn0_sum_q_syned; + logic [MEAS_WBP_WL-1:0] measurement_cn0_wbp; + logic [MEAS_WBP_WL-1:0] measurement_cn0_wbp_syned; + logic [MEAS_COUNT_WL-1:0] measurement_cn0_sample_count; + logic [MEAS_COUNT_WL-1:0] measurement_cn0_sample_count_syned; + logic [15:0] measurement_sequence, measurement_sequence_syned; logic preamble_ok; // ( internal flags ) @@ -77,6 +101,7 @@ module tracking_wrapper #( logic trck_nav_bit_rcv_flag_p, trck_nav_bit_rcv_flag_n; logic trck_timeout_flag_p, trck_timeout_flag_n; logic trck_nav_solve_flag_p, trck_nav_solve_flag_n; + logic trck_measurement_flag_p, trck_measurement_flag_n; logic [31 : 0] timeout_counter_p, timeout_counter_n; // tracking auto timeout value @@ -94,6 +119,24 @@ module tracking_wrapper #( logic [31 : 0] nav_solve_timer_reg_p, nav_solve_timer_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. + logic measurement_valid_r; + logic measurement_capture_pending; + logic [1:0] measurement_settle_count; + logic measurement_capture; + logic measurement_interrupt_pulse; + logic measurement_data_valid_reg; + logic measurement_bit_reg; + logic measurement_lock_reg; + logic [31:0] measurement_abs_sample_reg; + logic [REM_CODE_PH_I_WL-1:0] measurement_rem_code_ph_reg; + logic signed [MEAS_CARRIER_WL-1:0] measurement_carrier_freq_q7_reg; + logic signed [MEAS_SUM_WL-1:0] measurement_cn0_sum_i_reg; + logic signed [MEAS_SUM_WL-1:0] measurement_cn0_sum_q_reg; + logic [MEAS_WBP_WL-1:0] measurement_cn0_wbp_reg; + logic [MEAS_COUNT_WL-1:0] measurement_cn0_sample_count_reg; + logic [15:0] measurement_sequence_reg; + logic[2 : 0] start_trck_ctl_p, start_trck_ctl_n; logic[2 : 0] stop_trck_ctl_p, stop_trck_ctl_n; @@ -152,11 +195,55 @@ module tracking_wrapper #( .aresetn(PRESETN), .Din(rem_code_ph), .Dout(rem_code_ph_syned) - ); + ); + + double_ff_syn#(1, 2) i_measurement_valid_syn( + .clk(PCLK), .aresetn(PRESETN), + .Din(measurement_valid), .Dout(measurement_valid_syned) + ); + double_ff_syn#(1, 2) i_measurement_bit_syn( + .clk(PCLK), .aresetn(PRESETN), + .Din(measurement_bit), .Dout(measurement_bit_syned) + ); + double_ff_syn#(32, 2) i_measurement_abs_sample_syn( + .clk(PCLK), .aresetn(PRESETN), + .Din(measurement_abs_sample), .Dout(measurement_abs_sample_syned) + ); + double_ff_syn#(REM_CODE_PH_I_WL, 2) i_measurement_rem_code_ph_syn( + .clk(PCLK), .aresetn(PRESETN), + .Din(measurement_rem_code_ph), .Dout(measurement_rem_code_ph_syned) + ); + double_ff_syn#(MEAS_CARRIER_WL, 2) i_measurement_carrier_syn( + .clk(PCLK), .aresetn(PRESETN), + .Din(measurement_carrier_freq_q7), + .Dout(measurement_carrier_freq_q7_syned) + ); + double_ff_syn#(MEAS_SUM_WL, 2) i_measurement_sum_i_syn( + .clk(PCLK), .aresetn(PRESETN), + .Din(measurement_cn0_sum_i), .Dout(measurement_cn0_sum_i_syned) + ); + double_ff_syn#(MEAS_SUM_WL, 2) i_measurement_sum_q_syn( + .clk(PCLK), .aresetn(PRESETN), + .Din(measurement_cn0_sum_q), .Dout(measurement_cn0_sum_q_syned) + ); + double_ff_syn#(MEAS_WBP_WL, 2) i_measurement_wbp_syn( + .clk(PCLK), .aresetn(PRESETN), + .Din(measurement_cn0_wbp), .Dout(measurement_cn0_wbp_syned) + ); + double_ff_syn#(MEAS_COUNT_WL, 2) i_measurement_count_syn( + .clk(PCLK), .aresetn(PRESETN), + .Din(measurement_cn0_sample_count), + .Dout(measurement_cn0_sample_count_syned) + ); + double_ff_syn#(16, 2) i_measurement_sequence_syn( + .clk(PCLK), .aresetn(PRESETN), + .Din(measurement_sequence), .Dout(measurement_sequence_syned) + ); + trck_top i_trck_top( // common signals - .trk_rstn(PRESETN), - .afe_rstn(PRESETN), + .trk_rstn(trck_resetn), + .afe_rstn(afe_resetn), .trk_clk(trck_clk), .afe_clk(afe_clk), .sample_align_i(sample_align), // from afe @@ -173,15 +260,83 @@ module tracking_wrapper #( // output to regs. (all values are valid on bit_vld except lock ?) .bit_o(nav_bit), .bit_vld_o(bit_valid), - .abs_sample_o(abs_sample), // 20 ms bit valid aligned - .rem_code_ph_o(rem_code_ph), - .lock_o(lock), + .abs_sample_o(abs_sample), // 20 ms bit valid aligned + .rem_code_ph_o(rem_code_ph), + .measurement_vld_o(measurement_valid), + .measurement_bit_o(measurement_bit), + .measurement_abs_sample_o(measurement_abs_sample), + .measurement_rem_code_ph_o(measurement_rem_code_ph), + .measurement_carrier_freq_q7_o(measurement_carrier_freq_q7), + .measurement_cn0_sum_i_o(measurement_cn0_sum_i), + .measurement_cn0_sum_q_o(measurement_cn0_sum_q), + .measurement_cn0_wbp_o(measurement_cn0_wbp), + .measurement_cn0_sample_count_o(measurement_cn0_sample_count), + .measurement_sequence_o(measurement_sequence), + .lock_o(lock), .busy_o(busy), //unconnected ( for debug purposes ) .fifo_of_o() - ); + ); + + /* + * The source payload is stable for 20 ms. Its valid marker and every data + * bit pass through the same three-stage synchronizer; two extra PCLK cycles + * after the synchronized rising edge provide a conservative bus-settle + * margin before the software-visible registers and interrupt are updated. + */ + always_ff @(posedge PCLK or negedge PRESETN) begin : measurement_capture_seq + if(!PRESETN) begin + measurement_valid_r <= 1'b0; + measurement_capture_pending <= 1'b0; + measurement_settle_count <= '0; + measurement_capture <= 1'b0; + measurement_interrupt_pulse <= 1'b0; + measurement_data_valid_reg <= 1'b0; + measurement_bit_reg <= 1'b0; + measurement_lock_reg <= 1'b0; + measurement_abs_sample_reg <= '0; + measurement_rem_code_ph_reg <= '0; + measurement_carrier_freq_q7_reg <= '0; + measurement_cn0_sum_i_reg <= '0; + measurement_cn0_sum_q_reg <= '0; + measurement_cn0_wbp_reg <= '0; + measurement_cn0_sample_count_reg <= '0; + measurement_sequence_reg <= '0; + end else begin + measurement_valid_r <= measurement_valid_syned; + measurement_capture <= 1'b0; + measurement_interrupt_pulse <= measurement_capture; + + if(measurement_valid_syned && !measurement_valid_r) begin + measurement_capture_pending <= 1'b1; + measurement_settle_count <= 2; + end else if(measurement_capture_pending) begin + if(measurement_settle_count != 0) begin + measurement_settle_count <= measurement_settle_count - 1'b1; + end else begin + measurement_capture_pending <= 1'b0; + measurement_capture <= 1'b1; + measurement_data_valid_reg <= 1'b1; + measurement_bit_reg <= measurement_bit_syned; + measurement_lock_reg <= lock_syned; + measurement_abs_sample_reg <= measurement_abs_sample_syned; + measurement_rem_code_ph_reg <= measurement_rem_code_ph_syned; + measurement_carrier_freq_q7_reg <= + $signed(measurement_carrier_freq_q7_syned); + measurement_cn0_sum_i_reg <= + $signed(measurement_cn0_sum_i_syned); + measurement_cn0_sum_q_reg <= + $signed(measurement_cn0_sum_q_syned); + measurement_cn0_wbp_reg <= measurement_cn0_wbp_syned; + measurement_cn0_sample_count_reg <= + measurement_cn0_sample_count_syned; + measurement_sequence_reg <= measurement_sequence_syned; + end + end + end + end @@ -214,11 +369,13 @@ module tracking_wrapper #( trck_timeout_flag_p <= '0; trck_nav_solve_flag_p <= '0; trck_nav_bit_rcv_flag_p <= '0; + trck_measurement_flag_p <= '0; // control reg bit fields set by reset. control_reg_p[TRCK_NAV_BITS_FLAG_ENABLE_POS] <= 1'b1; control_reg_p[TRCK_NAV_SOLVE_FLAG_EN_POS] <= 1'b1; control_reg_p[TRCK_STOP_ON_UNLOCK_FLAG_EN_POS] <= 1'b1; + control_reg_p[TRCK_MEASUREMENT_FLAG_EN_POS] <= 1'b1; foreach(nav_bit_read_reg_p[i]) nav_bit_read_reg_p[i] <= '0; @@ -244,6 +401,7 @@ module tracking_wrapper #( trck_timeout_flag_p <= trck_timeout_flag_n; trck_nav_bit_rcv_flag_p <= trck_nav_bit_rcv_flag_n; trck_nav_solve_flag_p <= trck_nav_solve_flag_n; + trck_measurement_flag_p <= trck_measurement_flag_n; foreach(nav_bit_read_reg_p[i]) nav_bit_read_reg_p[i] <= nav_bit_read_reg_n[i]; end @@ -409,6 +567,10 @@ module tracking_wrapper #( if((control_reg_p[TRCK_NAV_SOLVE_INTR_EN_POS]) & (nav_solve_timer_reached)) begin gps_frame_ready = '1; end + // intr due to a coherent 20 ms Doppler/CN0 measurement. + if(control_reg_p[TRCK_MEASUREMENT_INTR_EN_POS] && measurement_interrupt_pulse) begin + gps_frame_ready = '1; + end end end @@ -419,6 +581,7 @@ module tracking_wrapper #( trck_timeout_flag_n = trck_timeout_flag_p; // (all internal clear signals are tied to generic int_clear signal) trck_nav_bit_rcv_flag_n = trck_nav_bit_rcv_flag_p; trck_nav_solve_flag_n = trck_nav_solve_flag_p; + trck_measurement_flag_n = trck_measurement_flag_p; foreach(nav_bit_read_reg_n[i]) nav_bit_read_reg_n[i] = nav_bit_read_reg_p[i]; @@ -443,12 +606,21 @@ module tracking_wrapper #( if(control_reg_p[TRCK_INTRPT_ENABLE_POS] && control_reg_p[TRCK_NAV_SOLVE_INTR_EN_POS]) intr_pending_n = '1; end + if(measurement_capture) begin + if(control_reg_p[TRCK_MEASUREMENT_FLAG_EN_POS]) + trck_measurement_flag_n = '1; + if(control_reg_p[TRCK_INTRPT_ENABLE_POS] && + control_reg_p[TRCK_MEASUREMENT_INTR_EN_POS]) + intr_pending_n = '1; + end + // generic clear all interrupt pending flag and other flags. if(intr_clear) begin // all internal flag clear intr_pending_n = '0; trck_timeout_flag_n = '0; trck_nav_bit_rcv_flag_n = '0; trck_nav_solve_flag_n = '0; + trck_measurement_flag_n = '0; end end : nav_bit_load_and_intr_flag_comb @@ -515,6 +687,10 @@ module tracking_wrapper #( if(PWDATA[TRCK_NAV_SOLVE_FLAG_EN_POS]) control_reg_n[TRCK_NAV_SOLVE_FLAG_EN_POS] = 1'b1; if(!PWDATA[TRCK_NAV_SOLVE_FLAG_EN_POS]) control_reg_n[TRCK_NAV_SOLVE_FLAG_EN_POS] = 1'b0; + if(PWDATA[TRCK_MEASUREMENT_INTR_EN_POS]) control_reg_n[TRCK_MEASUREMENT_INTR_EN_POS] = 1'b1; + 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; end (LOW_ADDRESS + PRN_NUMBER_REG_OFFSET): begin @@ -560,20 +736,23 @@ module tracking_wrapper #( (control_reg_p[TRCK_NAV_SOLVE_INTR_EN_POS] << trck_pkg::TRCK_NAV_SOLVE_INTR_EN_POS) | (control_reg_p[TRCK_NAV_BITS_FLAG_ENABLE_POS] << trck_pkg::TRCK_NAV_BITS_FLAG_ENABLE_POS) | (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_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) ); end (LOW_ADDRESS + STATUS_REG_OFFSET): begin PRDATA = 32'( - (busy_syned << TRCK_BUSY_POS) | - (lock_syned << TRCK_LOCKED_POS) | - (0 << TRCK_FIFO_OV_POS) | // fifo overflow is not considered for now. - (intr_pending_p << TRCK_INTR_PEND_POS) | - (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) - ); + (busy_syned << TRCK_BUSY_POS) | + (lock_syned << TRCK_LOCKED_POS) | + (0 << TRCK_FIFO_OV_POS) | // fifo overflow is not considered for now. + (intr_pending_p << TRCK_INTR_PEND_POS) | + (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) + ); end (LOW_ADDRESS + PRN_NUMBER_REG_OFFSET): @@ -653,6 +832,58 @@ module tracking_wrapper #( //PRDATA = rem_code_ph_reg_p; PRDATA = {{(32-REM_CODE_PH_I_WL){1'b0}}, rem_code_ph_reg_p}; end + (LOW_ADDRESS + MEASUREMENT_META_REG_OFFSET): + begin + PRDATA = { + 8'h01, + 11'b0, + measurement_cn0_sample_count_reg, + 5'b0, + measurement_lock_reg, + measurement_bit_reg, + measurement_data_valid_reg + }; + end + (LOW_ADDRESS + MEASUREMENT_SEQUENCE_REG_OFFSET): + begin + PRDATA = {16'b0, measurement_sequence_reg}; + end + (LOW_ADDRESS + MEASUREMENT_CARRIER_REG_OFFSET): + begin + PRDATA = {{(32-MEAS_CARRIER_WL){ + measurement_carrier_freq_q7_reg[MEAS_CARRIER_WL-1]}}, + measurement_carrier_freq_q7_reg}; + end + (LOW_ADDRESS + MEASUREMENT_ABS_SAMPLE_REG_OFFSET): + begin + PRDATA = measurement_abs_sample_reg; + end + (LOW_ADDRESS + MEASUREMENT_REM_CODE_REG_OFFSET): + begin + PRDATA = {{(32-REM_CODE_PH_I_WL){1'b0}}, + measurement_rem_code_ph_reg}; + end + (LOW_ADDRESS + MEASUREMENT_SUM_I_REG_OFFSET): + begin + PRDATA = {{(32-MEAS_SUM_WL){ + measurement_cn0_sum_i_reg[MEAS_SUM_WL-1]}}, + measurement_cn0_sum_i_reg}; + end + (LOW_ADDRESS + MEASUREMENT_SUM_Q_REG_OFFSET): + begin + PRDATA = {{(32-MEAS_SUM_WL){ + measurement_cn0_sum_q_reg[MEAS_SUM_WL-1]}}, + measurement_cn0_sum_q_reg}; + end + (LOW_ADDRESS + MEASUREMENT_WBP_LO_REG_OFFSET): + begin + PRDATA = measurement_cn0_wbp_reg[31:0]; + end + (LOW_ADDRESS + MEASUREMENT_WBP_HI_REG_OFFSET): + begin + PRDATA = {{(64-MEAS_WBP_WL){1'b0}}, + measurement_cn0_wbp_reg[MEAS_WBP_WL-1:32]}; + end endcase end end diff --git a/hardware/external/gps/rtl/apb_wrappers/trck_pkg.sv b/hardware/external/gps/rtl/apb_wrappers/trck_pkg.sv index c04c7f2..0e75700 100644 --- a/hardware/external/gps/rtl/apb_wrappers/trck_pkg.sv +++ b/hardware/external/gps/rtl/apb_wrappers/trck_pkg.sv @@ -21,7 +21,16 @@ package trck_pkg; NAV_DATA_9_REG_OFFSET = 60, TRCK_TIMEOUT_REG_OFFSET = 64, NAV_SOLVE_TIMER_REG_OFFSET = 68, - REM_CODE_PH_REG_OFFSET = 76 + REM_CODE_PH_REG_OFFSET = 76, + MEASUREMENT_META_REG_OFFSET = 80, + MEASUREMENT_SEQUENCE_REG_OFFSET = 84, + MEASUREMENT_CARRIER_REG_OFFSET = 88, + MEASUREMENT_ABS_SAMPLE_REG_OFFSET = 92, + MEASUREMENT_REM_CODE_REG_OFFSET = 96, + MEASUREMENT_SUM_I_REG_OFFSET = 100, + MEASUREMENT_SUM_Q_REG_OFFSET = 104, + MEASUREMENT_WBP_LO_REG_OFFSET = 108, + MEASUREMENT_WBP_HI_REG_OFFSET = 112 }trck_wrapper_reg_offets_e; @@ -39,7 +48,9 @@ package trck_pkg; TRCK_STOP_ON_UNLOCK_FLAG_EN_POS = 9, // enable pos. TRCK_NAV_SOLVE_ENABLE_POS = 10, // clear pos TRCK_NAV_SOLVE_INTR_EN_POS = 11, // clear pos - TRCK_NAV_SOLVE_FLAG_EN_POS = 12 // clear pos + TRCK_NAV_SOLVE_FLAG_EN_POS = 12, // clear pos + TRCK_MEASUREMENT_INTR_EN_POS = 13, + TRCK_MEASUREMENT_FLAG_EN_POS = 14 }trck_control_reg_pos_e; typedef enum @@ -50,7 +61,8 @@ package trck_pkg; TRCK_INTR_PEND_POS = 3, TRCK_NAV_BITS_RCV_FLAG_POS = 4, TRCK_UNLOCK_FLAG_POS = 5, - TRCK_NAV_SOLVE_FLAG_POS = 6 + TRCK_NAV_SOLVE_FLAG_POS = 6, + TRCK_MEASUREMENT_FLAG_POS = 7 }trck_status_reg_pos_e; localparam logic [7 : 0] PREAMBLE = 8'b10001011; diff --git a/hardware/external/gps/rtl/common/gps_top.sv b/hardware/external/gps/rtl/common/gps_top.sv index bfaf142..3e54379 100644 --- a/hardware/external/gps/rtl/common/gps_top.sv +++ b/hardware/external/gps/rtl/common/gps_top.sv @@ -105,6 +105,17 @@ generate .bit_o (trck_o[ch].bit_o), .bit_vld_o (trck_o[ch].bit_vld), .abs_sample_o (trck_o[ch].abs_sample), + .rem_code_ph_o (), + .measurement_vld_o (), + .measurement_bit_o (), + .measurement_abs_sample_o(), + .measurement_rem_code_ph_o(), + .measurement_carrier_freq_q7_o(), + .measurement_cn0_sum_i_o(), + .measurement_cn0_sum_q_o(), + .measurement_cn0_wbp_o(), + .measurement_cn0_sample_count_o(), + .measurement_sequence_o(), .fifo_of_o (), //reserved .lock_o (trck_o[ch].lock), .busy_o (trck_o[ch].busy) diff --git a/hardware/external/gps/rtl/filelist.f b/hardware/external/gps/rtl/filelist.f index 06d8bf9..848aa43 100644 --- a/hardware/external/gps/rtl/filelist.f +++ b/hardware/external/gps/rtl/filelist.f @@ -35,6 +35,7 @@ trck_src/cordic_atan_one_stage.sv trck_src/cos2x.sv trck_src/divider.sv trck_src/lock_detector.sv +trck_src/tracking_measurement.sv trck_src/trck_cntrl.sv trck_src/trck_top.sv trck_src/xil_fifo.sv diff --git a/hardware/external/gps/rtl/trck_src/lock_detector.sv b/hardware/external/gps/rtl/trck_src/lock_detector.sv index 1ca6b64..c244057 100644 --- a/hardware/external/gps/rtl/trck_src/lock_detector.sv +++ b/hardware/external/gps/rtl/trck_src/lock_detector.sv @@ -34,6 +34,8 @@ module lock_detector #( input logic corr_vld_i, input logic signed [IN_WL-1 : 0] i_p_i, input logic signed [IN_WL-1 : 0] q_p_i, + output logic prompt_energy_vld_o, + output logic [(2 * IN_WL):0] prompt_energy_o, output logic lock_o ); localparam DIV_OUT_IL = DIV_OUT_WL - DIV_OUT_FL; @@ -64,6 +66,8 @@ logic lock_decision_bad; assign i_prompt_abs = i_p_i[IN_WL-1] ? (~i_p_i + {{(IN_WL-1){1'b0}}, 1'b1}) : i_p_i; assign q_prompt_abs = q_p_i[IN_WL-1] ? (~q_p_i + {{(IN_WL-1){1'b0}}, 1'b1}) : q_p_i; assign prompt_energy_sum = {1'b0, i_prompt_energy_r} + {1'b0, q_prompt_energy_r}; +assign prompt_energy_vld_o = prompt_energy_vld; +assign prompt_energy_o = prompt_energy_sum; cos2x #( .IN_WL (IN_WL), diff --git a/hardware/external/gps/rtl/trck_src/tb/Makefile b/hardware/external/gps/rtl/trck_src/tb/Makefile new file mode 100644 index 0000000..205c54c --- /dev/null +++ b/hardware/external/gps/rtl/trck_src/tb/Makefile @@ -0,0 +1,26 @@ +QUESTA_VLOG ?= vlog +QUESTA_VSIM ?= vsim +QUESTA_RUN_DIR ?= /tmp/gps_tracking_measurement_questa + +RTL_DIR := $(abspath ../..) + +.PHONY: test clean + +test: + rm -rf $(QUESTA_RUN_DIR) + mkdir -p $(QUESTA_RUN_DIR) + cd $(QUESTA_RUN_DIR) && vlib work + cd $(RTL_DIR) && $(QUESTA_VLOG) -sv -work $(QUESTA_RUN_DIR)/work \ + -f filelist.f \ + $(CURDIR)/lock_detector_energy_tb.sv \ + $(CURDIR)/tracking_measurement_tb.sv \ + $(CURDIR)/tracking_wrapper_measurement_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' + +clean: + rm -rf $(QUESTA_RUN_DIR) diff --git a/hardware/external/gps/rtl/trck_src/tb/lock_detector_energy_tb.sv b/hardware/external/gps/rtl/trck_src/tb/lock_detector_energy_tb.sv new file mode 100644 index 0000000..e8dcf0f --- /dev/null +++ b/hardware/external/gps/rtl/trck_src/tb/lock_detector_energy_tb.sv @@ -0,0 +1,54 @@ +`timescale 1ns/1ps + +import definitions::*; + +module lock_detector_energy_tb; + logic clk = 1'b0; + logic rstn = 1'b0; + logic corr_valid = 1'b0; + logic signed [TRCK_CORR_WL-1:0] i_prompt = '0; + logic signed [TRCK_CORR_WL-1:0] q_prompt = '0; + logic energy_valid; + logic [(2*TRCK_CORR_WL):0] energy; + logic lock; + + always #4 clk = ~clk; + + lock_detector #( + .IN_WL(TRCK_CORR_WL), + .IN_FL(TRCK_CORR_FL), + .DIV_OUT_WL(LOCK_DET_DIV_O_WL), + .DIV_OUT_FL(LOCK_DET_DIV_O_FL), + .DIV_ITER(14) + ) dut ( + .rstn, + .clk, + .corr_vld_i(corr_valid), + .i_p_i(i_prompt), + .q_p_i(q_prompt), + .prompt_energy_vld_o(energy_valid), + .prompt_energy_o(energy), + .lock_o(lock) + ); + + initial begin + repeat(4) @(posedge clk); + rstn = 1'b1; + repeat(2) @(posedge clk); + + i_prompt = -22'sd1234; + q_prompt = 22'sd567; + corr_valid = 1'b1; + @(posedge clk); + corr_valid = 1'b0; + + wait(energy_valid); + #1; + if(energy !== ((1234 * 1234) + (567 * 567))) + $fatal(1, "prompt energy mismatch: got=%0d expected=%0d", + energy, ((1234 * 1234) + (567 * 567))); + + $display("lock_detector_energy_tb PASS"); + $finish; + end +endmodule diff --git a/hardware/external/gps/rtl/trck_src/tb/tracking_measurement_tb.sv b/hardware/external/gps/rtl/trck_src/tb/tracking_measurement_tb.sv new file mode 100644 index 0000000..80cadce --- /dev/null +++ b/hardware/external/gps/rtl/trck_src/tb/tracking_measurement_tb.sv @@ -0,0 +1,185 @@ +`timescale 1ns/1ps + +import definitions::*; + +module tracking_measurement_tb; + localparam int SUM_WL = TRCK_CORR_WL + $clog2(MS_PER_BIT); + localparam int WBP_WL = (2 * TRCK_CORR_WL) + 1 + $clog2(MS_PER_BIT); + localparam int COUNT_WL = $clog2(MS_PER_BIT + 1); + localparam int CARRIER_WL = ACQUIRED_FREQ_IL + TRCK_CARR_NCO_FREQ_FL; + + logic clk = 1'b0; + logic rstn = 1'b0; + logic bit_valid = 1'b0; + logic nav_bit = 1'b0; + logic [ABS_SAMPLE_CNTR_WL-1:0] absolute_sample = '0; + logic [REM_CODE_PH_I_WL-1:0] rem_code_phase = '0; + logic signed [CARRIER_WL-1:0] carrier_freq_q7 = '0; + logic corr_valid = 1'b0; + logic signed [TRCK_CORR_WL-1:0] i_prompt = '0; + logic signed [TRCK_CORR_WL-1:0] q_prompt = '0; + logic prompt_energy_valid = 1'b0; + logic [(2*TRCK_CORR_WL):0] prompt_energy = '0; + + logic measurement_valid; + logic measurement_bit; + logic [ABS_SAMPLE_CNTR_WL-1:0] measurement_abs_sample; + logic [REM_CODE_PH_I_WL-1:0] measurement_rem_code_phase; + logic signed [CARRIER_WL-1:0] measurement_carrier_freq_q7; + logic signed [SUM_WL-1:0] cn0_sum_i; + logic signed [SUM_WL-1:0] cn0_sum_q; + logic [WBP_WL-1:0] cn0_wbp; + logic [COUNT_WL-1:0] cn0_sample_count; + logic [15:0] sequence_value; + + integer expected_sum_i; + integer expected_sum_q; + longint unsigned expected_wbp; + + always #4 clk = ~clk; + + tracking_measurement dut ( + .clk, + .rstn, + .bit_valid_i(bit_valid), + .nav_bit_i(nav_bit), + .absolute_sample_i(absolute_sample), + .rem_code_phase_i(rem_code_phase), + .carrier_freq_q7_i(carrier_freq_q7), + .corr_valid_i(corr_valid), + .i_prompt_i(i_prompt), + .q_prompt_i(q_prompt), + .prompt_energy_valid_i(prompt_energy_valid), + .prompt_energy_i(prompt_energy), + .measurement_valid_o(measurement_valid), + .nav_bit_o(measurement_bit), + .absolute_sample_o(measurement_abs_sample), + .rem_code_phase_o(measurement_rem_code_phase), + .carrier_freq_q7_o(measurement_carrier_freq_q7), + .cn0_sum_i_o(cn0_sum_i), + .cn0_sum_q_o(cn0_sum_q), + .cn0_wbp_o(cn0_wbp), + .cn0_sample_count_o(cn0_sample_count), + .sequence_o(sequence_value) + ); + + task automatic clock_cycles(input int unsigned count); + repeat(count) @(posedge clk); + endtask + + task automatic pulse_bit_epoch(input logic value); + nav_bit = value; + bit_valid = 1'b1; + clock_cycles(12); + bit_valid = 1'b0; + clock_cycles(3); + endtask + + task automatic add_prompt(input integer i_value, input integer q_value); + longint unsigned energy; + energy = (i_value * i_value) + (q_value * q_value); + i_prompt = i_value; + q_prompt = q_value; + corr_valid = 1'b1; + @(posedge clk); + corr_valid = 1'b0; + @(posedge clk); + prompt_energy = energy; + prompt_energy_valid = 1'b1; + @(posedge clk); + prompt_energy_valid = 1'b0; + @(posedge clk); + expected_sum_i += i_value; + expected_sum_q += q_value; + expected_wbp += energy; + endtask + + task automatic check_measurement( + input logic expected_bit, + input logic [31:0] expected_abs, + input logic [REM_CODE_PH_I_WL-1:0] expected_rem, + input logic signed [CARRIER_WL-1:0] expected_carrier, + input logic [15:0] expected_sequence); + if(!measurement_valid) + $fatal(1, "measurement valid did not remain high"); + if(measurement_bit !== expected_bit) + $fatal(1, "NAV bit mismatch"); + if(measurement_abs_sample !== expected_abs) + $fatal(1, "absolute sample mismatch"); + if(measurement_rem_code_phase !== expected_rem) + $fatal(1, "remaining code phase mismatch"); + if(measurement_carrier_freq_q7 !== expected_carrier) + $fatal(1, "carrier Q7 mismatch: got=%0d expected=%0d", + measurement_carrier_freq_q7, expected_carrier); + if($signed(cn0_sum_i) !== expected_sum_i) + $fatal(1, "sum I mismatch: got=%0d expected=%0d", + $signed(cn0_sum_i), expected_sum_i); + if($signed(cn0_sum_q) !== expected_sum_q) + $fatal(1, "sum Q mismatch: got=%0d expected=%0d", + $signed(cn0_sum_q), expected_sum_q); + if(cn0_wbp !== expected_wbp) + $fatal(1, "WBP mismatch: got=%0d expected=%0d", cn0_wbp, expected_wbp); + if(cn0_sample_count !== MS_PER_BIT) + $fatal(1, "sample count mismatch: got=%0d", cn0_sample_count); + if(sequence_value !== expected_sequence) + $fatal(1, "sequence mismatch: got=%0d expected=%0d", + sequence_value, expected_sequence); + endtask + + initial begin + clock_cycles(4); + rstn = 1'b1; + clock_cycles(2); + + /* First edge only establishes the 20 ms integration phase. */ + pulse_bit_epoch(1'b0); + + expected_sum_i = 0; + expected_sum_q = 0; + expected_wbp = 0; + for(integer i = 0; i < MS_PER_BIT; i++) begin + add_prompt(100 + i, -20 + i); + end + + nav_bit = 1'b1; + absolute_sample = 32'h1234_5678; + rem_code_phase = REM_CODE_PH_I_WL'(321); + carrier_freq_q7 = CARRIER_WL'((CENTER_FREQ + 1250) * 128); + bit_valid = 1'b1; + clock_cycles(10); + check_measurement( + 1'b1, 32'h1234_5678, REM_CODE_PH_I_WL'(321), + CARRIER_WL'((CENTER_FREQ + 1250) * 128), 16'd1); + + /* Payload inputs may move; the registered measurement must not. */ + carrier_freq_q7 = CARRIER_WL'((CENTER_FREQ - 3000) * 128); + absolute_sample = 32'hffff_0000; + clock_cycles(5); + check_measurement( + 1'b1, 32'h1234_5678, REM_CODE_PH_I_WL'(321), + CARRIER_WL'((CENTER_FREQ + 1250) * 128), 16'd1); + bit_valid = 1'b0; + clock_cycles(3); + if(measurement_valid) + $fatal(1, "measurement valid failed to deassert"); + + expected_sum_i = 0; + expected_sum_q = 0; + expected_wbp = 0; + for(integer j = 0; j < MS_PER_BIT; j++) begin + add_prompt(-50, 25); + end + nav_bit = 1'b0; + absolute_sample = 32'h2345_6789; + rem_code_phase = REM_CODE_PH_I_WL'(777); + carrier_freq_q7 = CARRIER_WL'((CENTER_FREQ - 3000) * 128); + bit_valid = 1'b1; + clock_cycles(10); + check_measurement( + 1'b0, 32'h2345_6789, REM_CODE_PH_I_WL'(777), + CARRIER_WL'((CENTER_FREQ - 3000) * 128), 16'd2); + + $display("tracking_measurement_tb PASS"); + $finish; + end +endmodule diff --git a/hardware/external/gps/rtl/trck_src/tb/tracking_wrapper_measurement_tb.sv b/hardware/external/gps/rtl/trck_src/tb/tracking_wrapper_measurement_tb.sv new file mode 100644 index 0000000..08a18e7 --- /dev/null +++ b/hardware/external/gps/rtl/trck_src/tb/tracking_wrapper_measurement_tb.sv @@ -0,0 +1,172 @@ +`timescale 1ns/1ps + +import definitions::*; +import trck_pkg::*; + +module tracking_wrapper_measurement_tb; + localparam int CARRIER_WL = ACQUIRED_FREQ_IL + TRCK_CARR_NCO_FREQ_FL; + localparam int SUM_WL = TRCK_CORR_WL + $clog2(MS_PER_BIT); + localparam int WBP_WL = (2 * TRCK_CORR_WL) + 1 + $clog2(MS_PER_BIT); + localparam logic [29:0] EXPECTED_CARRIER = + CARRIER_WL'((CENTER_FREQ + 2500) * 128); + localparam logic [WBP_WL-1:0] EXPECTED_WBP = + WBP_WL'(50'h2_1234_89ab_cdef); + + 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; + + always #5 PCLK = ~PCLK; + always #4 trck_clk = ~trck_clk; + always #8 afe_clk = ~afe_clk; + always @(posedge PCLK) + if(gps_frame_ready) interrupt_count++; + + 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 + + initial begin + force dut.measurement_valid = 1'b0; + force dut.measurement_bit = 1'b1; + force dut.measurement_abs_sample = 32'h89ab_cdef; + force dut.measurement_rem_code_ph = REM_CODE_PH_I_WL'(555); + force dut.measurement_carrier_freq_q7 = EXPECTED_CARRIER; + force dut.measurement_cn0_sum_i = SUM_WL'(-12345); + force dut.measurement_cn0_sum_q = SUM_WL'(6789); + force dut.measurement_cn0_wbp = EXPECTED_WBP; + force dut.measurement_cn0_sample_count = $clog2(MS_PER_BIT+1)'(20); + force dut.measurement_sequence = 16'h55aa; + force dut.lock = 1'b1; + force dut.busy = 1'b1; + force dut.bit_valid = 1'b0; + + pclk_cycles(5); + PRESETN = 1'b1; + trck_resetn = 1'b1; + afe_resetn = 1'b1; + pclk_cycles(5); + + apb_write(CONTROL_REG_OFFSET[11:0], + (1 << TRCK_INTRPT_ENABLE_POS) | + (1 << TRCK_MEASUREMENT_INTR_EN_POS) | + (1 << TRCK_MEASUREMENT_FLAG_EN_POS)); + + @(negedge trck_clk); + force dut.measurement_valid = 1'b1; + pclk_cycles(12); + if(interrupt_count !== 1) + $fatal(1, "expected one measurement interrupt, got %0d", interrupt_count); + + expect_read(MEASUREMENT_META_REG_OFFSET[11:0], + {8'h01, 11'b0, 5'd20, 5'b0, 1'b1, 1'b1, 1'b1}); + expect_read(MEASUREMENT_SEQUENCE_REG_OFFSET[11:0], 32'h0000_55aa); + expect_read(MEASUREMENT_CARRIER_REG_OFFSET[11:0], + {{2{EXPECTED_CARRIER[29]}}, EXPECTED_CARRIER}); + expect_read(MEASUREMENT_ABS_SAMPLE_REG_OFFSET[11:0], 32'h89ab_cdef); + expect_read(MEASUREMENT_REM_CODE_REG_OFFSET[11:0], 32'd555); + expect_read(MEASUREMENT_SUM_I_REG_OFFSET[11:0], -32'sd12345); + expect_read(MEASUREMENT_SUM_Q_REG_OFFSET[11:0], 32'd6789); + expect_read(MEASUREMENT_WBP_LO_REG_OFFSET[11:0], EXPECTED_WBP[31:0]); + expect_read(MEASUREMENT_WBP_HI_REG_OFFSET[11:0], + {{(64-WBP_WL){1'b0}}, EXPECTED_WBP[WBP_WL-1:32]}); + + /* Source payload movement cannot tear the APB-domain snapshot. */ + force dut.measurement_carrier_freq_q7 = + CARRIER_WL'((CENTER_FREQ - 4000) * 128); + force dut.measurement_cn0_sum_i = SUM_WL'(22222); + pclk_cycles(4); + expect_read(MEASUREMENT_CARRIER_REG_OFFSET[11:0], + {{2{EXPECTED_CARRIER[29]}}, EXPECTED_CARRIER}); + expect_read(MEASUREMENT_SUM_I_REG_OFFSET[11:0], -32'sd12345); + + expect_read(STATUS_REG_OFFSET[11:0], + (1 << TRCK_BUSY_POS) | + (1 << TRCK_LOCKED_POS) | + (1 << TRCK_INTR_PEND_POS) | + (1 << TRCK_MEASUREMENT_FLAG_POS)); + apb_write(CONTROL_REG_OFFSET[11:0], + (1 << TRCK_INTRPT_ENABLE_POS) | + (1 << TRCK_INTRPT_CLEAR_POS) | + (1 << TRCK_MEASUREMENT_INTR_EN_POS) | + (1 << TRCK_MEASUREMENT_FLAG_EN_POS)); + expect_read(STATUS_REG_OFFSET[11:0], + (1 << TRCK_BUSY_POS) | (1 << TRCK_LOCKED_POS)); + + $display("tracking_wrapper_measurement_tb PASS"); + $finish; + end +endmodule diff --git a/hardware/external/gps/rtl/trck_src/tracking_measurement.sv b/hardware/external/gps/rtl/trck_src/tracking_measurement.sv new file mode 100644 index 0000000..9a23f8a --- /dev/null +++ b/hardware/external/gps/rtl/trck_src/tracking_measurement.sv @@ -0,0 +1,142 @@ +import definitions::*; + +/* + * Builds one coherent tracking measurement for every decoded 20 ms NAV bit. + * + * The payload is registered in the tracking clock domain and remains unchanged + * until the next measurement. measurement_valid_o remains asserted for the + * rest of bit_valid_i so the APB clock can synchronize it without a CDC FIFO. + */ +module tracking_measurement #( + parameter int unsigned CORR_WL = TRCK_CORR_WL, + parameter int unsigned ENERGY_WL = (2 * TRCK_CORR_WL) + 1, + parameter int unsigned SUM_WL = TRCK_CORR_WL + $clog2(MS_PER_BIT), + parameter int unsigned WBP_WL = ENERGY_WL + $clog2(MS_PER_BIT), + parameter int unsigned COUNT_WL = $clog2(MS_PER_BIT + 1), + parameter int unsigned CARRIER_WL = + ACQUIRED_FREQ_IL + TRCK_CARR_NCO_FREQ_FL, + parameter int unsigned SNAPSHOT_DELAY_CYCLES = 4 +) ( + input logic clk, + input logic rstn, + input logic bit_valid_i, + input logic nav_bit_i, + input logic [ABS_SAMPLE_CNTR_WL-1:0] absolute_sample_i, + input logic [REM_CODE_PH_I_WL-1:0] rem_code_phase_i, + input logic signed [CARRIER_WL-1:0] carrier_freq_q7_i, + input logic corr_valid_i, + input logic signed [CORR_WL-1:0] i_prompt_i, + input logic signed [CORR_WL-1:0] q_prompt_i, + input logic prompt_energy_valid_i, + input logic [ENERGY_WL-1:0] prompt_energy_i, + + output logic measurement_valid_o, + output logic nav_bit_o, + output logic [ABS_SAMPLE_CNTR_WL-1:0] absolute_sample_o, + output logic [REM_CODE_PH_I_WL-1:0] rem_code_phase_o, + output logic signed [CARRIER_WL-1:0] carrier_freq_q7_o, + output logic signed [SUM_WL-1:0] cn0_sum_i_o, + output logic signed [SUM_WL-1:0] cn0_sum_q_o, + output logic [WBP_WL-1:0] cn0_wbp_o, + output logic [COUNT_WL-1:0] cn0_sample_count_o, + output logic [15:0] sequence_o +); + +localparam int unsigned DELAY_WL = + (SNAPSHOT_DELAY_CYCLES < 2) ? 1 : $clog2(SNAPSHOT_DELAY_CYCLES + 1); + +logic bit_valid_r; +logic measurement_armed; +logic snapshot_pending; +logic [DELAY_WL-1:0] snapshot_delay; +logic signed [SUM_WL-1:0] sum_i; +logic signed [SUM_WL-1:0] sum_q; +logic [WBP_WL-1:0] wbp; +logic [COUNT_WL-1:0] sample_count; + +wire bit_epoch = bit_valid_i && !bit_valid_r; + +always_ff @(posedge clk or negedge rstn) begin + if(!rstn) begin + bit_valid_r <= 1'b0; + measurement_armed <= 1'b0; + snapshot_pending <= 1'b0; + snapshot_delay <= '0; + sum_i <= '0; + sum_q <= '0; + wbp <= '0; + sample_count <= '0; + measurement_valid_o <= 1'b0; + nav_bit_o <= 1'b0; + absolute_sample_o <= '0; + rem_code_phase_o <= '0; + carrier_freq_q7_o <= '0; + cn0_sum_i_o <= '0; + cn0_sum_q_o <= '0; + cn0_wbp_o <= '0; + cn0_sample_count_o <= '0; + sequence_o <= '0; + end else begin + bit_valid_r <= bit_valid_i; + + if(!bit_valid_i) begin + measurement_valid_o <= 1'b0; + end + + if(measurement_armed && corr_valid_i) begin + sum_i <= sum_i + $signed(i_prompt_i); + sum_q <= sum_q + $signed(q_prompt_i); + sample_count <= sample_count + 1'b1; + end + if(measurement_armed && prompt_energy_valid_i) begin + wbp <= wbp + prompt_energy_i; + end + + /* + * The first synchronized NAV-bit edge defines the integration phase. + * Samples gathered before that edge are intentionally discarded. + */ + if(bit_epoch) begin + if(!measurement_armed) begin + measurement_armed <= 1'b1; + sum_i <= '0; + sum_q <= '0; + wbp <= '0; + sample_count <= '0; + end else begin + snapshot_pending <= 1'b1; + snapshot_delay <= DELAY_WL'(SNAPSHOT_DELAY_CYCLES); + end + end + + /* + * Prompt energy is pipelined in lock_detector. There is almost one + * millisecond before the next correlator result, so waiting a few + * tracking clocks includes the last energy term without mixing epochs. + */ + if(snapshot_pending) begin + if(snapshot_delay != 0) begin + snapshot_delay <= snapshot_delay - 1'b1; + end else begin + snapshot_pending <= 1'b0; + measurement_valid_o <= 1'b1; + nav_bit_o <= nav_bit_i; + absolute_sample_o <= absolute_sample_i; + rem_code_phase_o <= rem_code_phase_i; + carrier_freq_q7_o <= carrier_freq_q7_i; + cn0_sum_i_o <= sum_i; + cn0_sum_q_o <= sum_q; + cn0_wbp_o <= wbp; + cn0_sample_count_o <= sample_count; + sequence_o <= sequence_o + 1'b1; + + sum_i <= '0; + sum_q <= '0; + wbp <= '0; + sample_count <= '0; + end + end + end +end + +endmodule diff --git a/hardware/external/gps/rtl/trck_src/trck_top.sv b/hardware/external/gps/rtl/trck_src/trck_top.sv index 10f9d7e..51c2381 100644 --- a/hardware/external/gps/rtl/trck_src/trck_top.sv +++ b/hardware/external/gps/rtl/trck_src/trck_top.sv @@ -34,6 +34,27 @@ module trck_top output logic bit_vld_o, output logic [ABS_SAMPLE_CNTR_WL-1 : 0] abs_sample_o, output logic [REM_CODE_PH_I_WL-1 : 0] rem_code_ph_o, + + // Coherent 20 ms tracking measurement (held until the next NAV bit) + output logic measurement_vld_o, + output logic measurement_bit_o, + output logic [ABS_SAMPLE_CNTR_WL-1 : 0] measurement_abs_sample_o, + output logic [REM_CODE_PH_I_WL-1 : 0] measurement_rem_code_ph_o, + output logic signed [ + ACQUIRED_FREQ_IL+TRCK_CARR_NCO_FREQ_FL-1 : 0 + ] measurement_carrier_freq_q7_o, + output logic signed [ + TRCK_CORR_WL+$clog2(MS_PER_BIT)-1 : 0 + ] measurement_cn0_sum_i_o, + output logic signed [ + TRCK_CORR_WL+$clog2(MS_PER_BIT)-1 : 0 + ] measurement_cn0_sum_q_o, + output logic [ + (2*TRCK_CORR_WL)+1+$clog2(MS_PER_BIT)-1 : 0 + ] measurement_cn0_wbp_o, + output logic [$clog2(MS_PER_BIT+1)-1 : 0] + measurement_cn0_sample_count_o, + output logic [15:0] measurement_sequence_o, // Status signals output logic fifo_of_o, @@ -77,6 +98,8 @@ logic ca_l, ca_p, ca_e; logic corr_vld; logic signed [TRCK_CORR_WL-TRCK_CORR_FL-1 : -TRCK_CORR_FL] i_e, i_p, i_l, q_e, q_p, q_l; +logic prompt_energy_vld; +logic [(2*TRCK_CORR_WL):0] prompt_energy; logic signed [RF_DATA_WIDTH-1 : 0] rf_data; @@ -223,7 +246,8 @@ Disc_PLL_i ( .y_i (q_p), .vld_o (pll_error_vld), - .angle_o (pll_error) + .angle_o (pll_error), + .vec_len_o() ); code_loop_discriminator #( @@ -259,6 +283,8 @@ lock_detector_i ( .corr_vld_i (corr_vld), .i_p_i (i_p), .q_p_i (q_p), + .prompt_energy_vld_o(prompt_energy_vld), + .prompt_energy_o(prompt_energy), .lock_o (lock_o) ); @@ -276,6 +302,31 @@ bit_gen bit_gen_i ( .rem_code_ph_o (rem_code_ph_o) ); +tracking_measurement tracking_measurement_i ( + .clk (trk_clk), + .rstn (rstn), + .bit_valid_i (bit_vld_o), + .nav_bit_i (bit_o), + .absolute_sample_i (abs_sample_o), + .rem_code_phase_i (rem_code_ph_o), + .carrier_freq_q7_i (carr_freq_sum_p), + .corr_valid_i (corr_vld), + .i_prompt_i (i_p), + .q_prompt_i (q_p), + .prompt_energy_valid_i (prompt_energy_vld), + .prompt_energy_i (prompt_energy), + .measurement_valid_o (measurement_vld_o), + .nav_bit_o (measurement_bit_o), + .absolute_sample_o (measurement_abs_sample_o), + .rem_code_phase_o (measurement_rem_code_ph_o), + .carrier_freq_q7_o (measurement_carrier_freq_q7_o), + .cn0_sum_i_o (measurement_cn0_sum_i_o), + .cn0_sum_q_o (measurement_cn0_sum_q_o), + .cn0_wbp_o (measurement_cn0_wbp_o), + .cn0_sample_count_o (measurement_cn0_sample_count_o), + .sequence_o (measurement_sequence_o) +); + //freq valid always_ff @(posedge trk_clk or negedge rstn) begin if (!rstn) begin diff --git a/soc/software/gps_system/Makefile b/soc/software/gps_system/Makefile index a852ea4..aaa9ab5 100644 --- a/soc/software/gps_system/Makefile +++ b/soc/software/gps_system/Makefile @@ -71,6 +71,7 @@ APP_SRCS := \ src/gps/gps_time_anchor.c \ src/gps/gps_nav_epoch.c \ src/gps/gps_pps.c \ + src/gps/gps_tracking_measurement.c \ src/gps/linked_list.c \ src/gps/NavEquations.c \ src/gps/matrixLib.c diff --git a/soc/software/gps_system/header/fusion/fusion_measurement.h b/soc/software/gps_system/header/fusion/fusion_measurement.h index 06086bd..9a08dc9 100644 --- a/soc/software/gps_system/header/fusion/fusion_measurement.h +++ b/soc/software/gps_system/header/fusion/fusion_measurement.h @@ -21,8 +21,12 @@ typedef struct { double vacc_m; double residual_rms_m; double chi_square_ratio; + double velocity_ecef_mps[3]; + double speed_mps; + double velocity_residual_rms_mps; unsigned int used_satellites; bool solution_valid; + bool velocity_valid; } fusion_gps_measurement_t; /* Timestamped IMU plus the independently supervised platform state consumed diff --git a/soc/software/gps_system/header/gps/NavEquations.h b/soc/software/gps_system/header/gps/NavEquations.h index e39863f..782e4ef 100644 --- a/soc/software/gps_system/header/gps/NavEquations.h +++ b/soc/software/gps_system/header/gps/NavEquations.h @@ -9,6 +9,10 @@ typedef struct gps_nav_solution { double ecef_m[3]; double clock_bias_m; + double velocity_ecef_mps[3]; + double clock_drift_mps; + double speed_mps; + double velocity_residual_rms_mps; double latitude_deg; double longitude_deg; double height_m; @@ -25,6 +29,9 @@ double postfit_residual_m[GPS_TRCK_IP_COUNT]; double elevation_deg[GPS_TRCK_IP_COUNT]; double measurement_sigma_m[GPS_TRCK_IP_COUNT]; + double cn0_dbhz[GPS_TRCK_IP_COUNT]; + double doppler_hz[GPS_TRCK_IP_COUNT]; + double pseudorange_rate_mps[GPS_TRCK_IP_COUNT]; double full_postfit_residual_m[GPS_TRCK_IP_COUNT]; double full_elevation_deg[GPS_TRCK_IP_COUNT]; double full_measurement_sigma_m[GPS_TRCK_IP_COUNT]; @@ -55,10 +62,11 @@ bool chi_square_pass; bool fde_applied; bool solution_valid; + bool velocity_valid; } gps_nav_solution_t; void satpos(gps_ephemeris_t* eph, double* pos, double* vel, double* clk, double* clockCorr); - void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_ephemeris_t* eph, ionoutc_t* ionoutc, double* xyz, t_temp_pos* tempPos, double* clockCorr, mat_type *satellitePositions, mat_type * velocities); + void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_ephemeris_t* eph, ionoutc_t* ionoutc, double* xyz, t_temp_pos* tempPos, double* clockCorr, double* clockDrift, mat_type *satellitePositions, mat_type * velocities); double check_t(long double tkl); void subVect(double* y, const double* x1, const double* x2); double normVect(const double* x); @@ -66,6 +74,7 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber, mat_type * az, mat_type * el, const unsigned char* ura_indices, + const double* cn0_dbhz, const ionoutc_t* ionoutc, double gps_tow, gps_nav_solution_t* solution_out); void rotateSatelliteSTD(double const traveltime, const mat_type* X_sat, mat_type* X_sat_rot); 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 fd01277..4599b4f 100644 --- a/soc/software/gps_system/header/gps/gps_interrupt_handlers.h +++ b/soc/software/gps_system/header/gps/gps_interrupt_handlers.h @@ -2,6 +2,7 @@ #define GPS_INT_HAND #include "gps/gps_ip_defines.h" + #include "gps/gps_tracking_measurement.h" typedef struct { uint32_t event_overflow_total; @@ -12,6 +13,8 @@ uint32_t nav_epoch_incomplete_total; uint32_t nav_epoch_snapshot_overwrite_total; uint32_t how_anchor_rejected_total; + uint32_t measurement_missed_total; + uint32_t measurement_invalid_total; uint32_t satellite_connected_total; uint32_t satellite_lost_total; uint32_t ephemeris_cache_store_total; @@ -32,6 +35,7 @@ uint64_t pps_counter[GPS_TRCK_IP_COUNT]; uint32_t pps_capture_sequence[GPS_TRCK_IP_COUNT]; uint64_t local_timestamp_ticks[GPS_TRCK_IP_COUNT]; + gps_tracking_measurement_t measurement[GPS_TRCK_IP_COUNT]; } gps_nav_epoch_snapshot_t; typedef void (*gps_external_irq_handler_t)(void); 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 1744487..549aa01 100644 --- a/soc/software/gps_system/header/gps/gps_ip_defines.h +++ b/soc/software/gps_system/header/gps/gps_ip_defines.h @@ -138,6 +138,8 @@ #define GPS_TRCK_NAV_SOLVE_EN_POS 10 #define GPS_TRCK_NAV_SOLVE_INTR_EN_POS 11 #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 // acq ip status reg bits. @@ -152,6 +154,18 @@ #define GPS_TRCK_NAV_BITS_RCV_POS 4 #define GPS_TRCK_STOP_UNLOCK_POS 5 #define GPS_TRCK_NAV_SOLVE_FLAG_POS 6 + #define GPS_TRCK_MEASUREMENT_FLAG_POS 7 + + #define GPS_TRCK_MEAS_META_VALID_POS 0 + #define GPS_TRCK_MEAS_META_NAV_BIT_POS 1 + #define GPS_TRCK_MEAS_META_LOCK_POS 2 + #define GPS_TRCK_MEAS_META_COUNT_SHIFT 8 + #define GPS_TRCK_MEAS_META_COUNT_MASK 0x1fu + #define GPS_TRCK_MEAS_META_VERSION_SHIFT 24 + #define GPS_TRCK_MEAS_META_VERSION 1u + #define GPS_TRCK_CARRIER_Q7_SCALE 128.0 + #define GPS_TRCK_IF_CENTER_HZ 4092000.0 + #define GPS_L1_CARRIER_HZ 1575420000.0 @@ -232,6 +246,15 @@ uint32_t NAV_SOLVE_TIMER; // 'h44 uint32_t RESERVED_48; // 'h48 uint32_t REM_CODE_PHASE_REG; // 'h4c + uint32_t MEASUREMENT_META; // 'h50 + uint32_t MEASUREMENT_SEQUENCE; // 'h54 + int32_t MEASUREMENT_CARRIER_Q7; // 'h58 + uint32_t MEASUREMENT_ABS_SAMPLE; // 'h5c + uint32_t MEASUREMENT_REM_CODE_PHASE; // 'h60 + int32_t MEASUREMENT_SUM_I; // 'h64 + int32_t MEASUREMENT_SUM_Q; // 'h68 + uint32_t MEASUREMENT_WBP_LO; // 'h6c + uint32_t MEASUREMENT_WBP_HI; // 'h70 }gps_trck_t; typedef enum { @@ -260,6 +283,9 @@ _Static_assert(offsetof(gps_trck_t, PRN_NUMBER) == 0x04, "gps_trck_t PRN offset mismatch"); _Static_assert(offsetof(gps_trck_t, STATUS_REG) == 0x10, "gps_trck_t STATUS offset mismatch"); _Static_assert(offsetof(gps_trck_t, DOPPLER_REG) == 0x14, "gps_trck_t DOPPLER offset mismatch"); + _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"); #endif @@ -442,6 +468,10 @@ void trck_enable_nav_bits_flag(volatile gps_trck_t* trck_ip); // enable nav bits internal flag generation. void trck_enable_nav_bits_intr(volatile gps_trck_t* trck_ip); // enable nav bits intr generation. void trck_disable_nav_bits_intr(volatile gps_trck_t* trck_ip); // disable nav bits intr generation. + void trck_enable_measurement_intr(volatile gps_trck_t* trck_ip); + void trck_disable_measurement_intr(volatile gps_trck_t* trck_ip); + void trck_enable_measurement_flag(volatile gps_trck_t* trck_ip); + void trck_disable_measurement_flag(volatile gps_trck_t* trck_ip); void trck_set_nav_solve_timer(unsigned int tick, volatile gps_trck_t* trck_ip); // in case of gps, 1 tick is 20 ms. diff --git a/soc/software/gps_system/header/gps/gps_tracking_measurement.h b/soc/software/gps_system/header/gps/gps_tracking_measurement.h new file mode 100644 index 0000000..74918cf --- /dev/null +++ b/soc/software/gps_system/header/gps/gps_tracking_measurement.h @@ -0,0 +1,61 @@ +#ifndef GPS_TRACKING_MEASUREMENT_H +#define GPS_TRACKING_MEASUREMENT_H + +#include +#include + +#include "gps/gps_ip_defines.h" + +typedef struct { + uint32_t prn; + uint32_t sequence; + uint32_t absolute_sample; + uint32_t remaining_code_phase; + uint64_t local_timestamp_ticks; + int32_t carrier_freq_q7; + int32_t cn0_sum_i; + int32_t cn0_sum_q; + uint64_t cn0_wbp; + unsigned int cn0_sample_count; + double doppler_hz; + double pseudorange_rate_mps; + double cn0_dbhz; + bool nav_bit; + bool locked; + bool valid; +} gps_tracking_measurement_t; + +/* + * Decode a coherent hardware mailbox. The sequence-before/after check protects + * callers that are not running directly in the measurement ISR. + */ +bool gps_tracking_measurement_read( + volatile const gps_trck_t* tracking_ip, + gps_tracking_measurement_t* measurement_out); + +bool gps_tracking_cn0_dbhz( + int32_t sum_i, + int32_t sum_q, + uint64_t wideband_power, + unsigned int coherent_samples, + double* cn0_dbhz_out); + +double gps_tracking_doppler_hz(int32_t carrier_freq_q7); +double gps_tracking_doppler_to_range_rate_mps(double doppler_hz); + +/* + * Four-state WLS: receiver ECEF velocity (m/s) and receiver clock drift + * (m/s). Only valid/locked measurements with finite C/N0 are used. + */ +bool gps_tracking_velocity_wls( + const double receiver_ecef_m[3], + const double satellite_positions_m[][3], + const double satellite_velocities_mps[][3], + const double satellite_clock_drifts_sps[], + const gps_tracking_measurement_t measurements[], + unsigned int count, + double receiver_velocity_ecef_mps_out[3], + double* receiver_clock_drift_mps_out, + double* residual_rms_mps_out); + +#endif diff --git a/soc/software/gps_system/src/adapters/gps_fusion_publisher.c b/soc/software/gps_system/src/adapters/gps_fusion_publisher.c index 0f33bc0..75ffc87 100644 --- a/soc/software/gps_system/src/adapters/gps_fusion_publisher.c +++ b/soc/software/gps_system/src/adapters/gps_fusion_publisher.c @@ -24,7 +24,14 @@ bool gps_fusion_build_measurement( measurement_out->vacc_m = solution->vacc_m; measurement_out->residual_rms_m = solution->residual_rms_m; measurement_out->chi_square_ratio = solution->chi_square_ratio; + memcpy(measurement_out->velocity_ecef_mps, + solution->velocity_ecef_mps, + sizeof(measurement_out->velocity_ecef_mps)); + measurement_out->speed_mps = solution->speed_mps; + measurement_out->velocity_residual_rms_mps = + solution->velocity_residual_rms_mps; measurement_out->used_satellites = solution->used_satellite_count; measurement_out->solution_valid = solution->solution_valid; + measurement_out->velocity_valid = solution->velocity_valid; return true; } diff --git a/soc/software/gps_system/src/adapters/gps_system_uart.c b/soc/software/gps_system/src/adapters/gps_system_uart.c index cf06c82..85350fa 100644 --- a/soc/software/gps_system/src/adapters/gps_system_uart.c +++ b/soc/software/gps_system/src/adapters/gps_system_uart.c @@ -27,10 +27,13 @@ void gps_system_uart_publish_location( GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION, "source=%s lat=%.20lf lon=%.20lf height=%.3lf " "gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf " - "tracked=%u used=%u\r\n", + "tracked=%u used=%u velocity_valid=%u speed=%.3lf " + "velocity_rms=%.3lf\r\n", source, latitude_deg, longitude_deg, height_m, gps->gdop, gps->pdop, gps->hdop, gps->vdop, gps->tdop, - gps->tracked_satellite_count, gps->used_satellite_count); + gps->tracked_satellite_count, gps->used_satellite_count, + gps->velocity_valid ? 1u : 0u, + gps->speed_mps, gps->velocity_residual_rms_mps); } void gps_system_uart_publish_fused_location( diff --git a/soc/software/gps_system/src/gps/NavEquations.c b/soc/software/gps_system/src/gps/NavEquations.c index 199c989..fc699dd 100644 --- a/soc/software/gps_system/src/gps/NavEquations.c +++ b/soc/software/gps_system/src/gps/NavEquations.c @@ -17,7 +17,7 @@ Returns: rho - [out] computed range ( pseudo range) */ -void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_ephemeris_t* eph, ionoutc_t* ionoutc, double* xyz, t_temp_pos* tempPos, double* clockCorr, mat_type *satellitePositions, mat_type * velocities){ +void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_ephemeris_t* eph, ionoutc_t* ionoutc, double* xyz, t_temp_pos* tempPos, double* clockCorr, double* clockDrift, mat_type *satellitePositions, mat_type * velocities){ double pos[3], vel[3], clk[2]; double los[3]; @@ -28,7 +28,7 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph //gpstime_t * gpstime = (gpstime_t*) malloc(sizeof(gpstime_t)); //gpstime->sec = eph->toe.sec; //gpstime->week = eph->toe.week; - + //CPP'ye GOREydi, KAPATILDI// double abs_sample[SVN] = { 66564551., 66525117.,66589459.,66666672.,66633917.,66700000. }; //CPP'ye GOREydi, KAPATILDI// int codePhase = 2772; // TODO should be given by tracker. Şu anda matlab'dan alindi.. @@ -38,6 +38,10 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph for (i = 0; i < 3; i++) { satellitePositions->matrix[sat_no][i] = pos[i]; + velocities->matrix[sat_no][i] = vel[i]; + } + if(clockDrift != NULL) { + *clockDrift = clk[1]; } /*for (i = 0; i < 3; i++) tempPos->sv_po_at_pseudo_obs[i] = pos[i]; //memset(los, '0', sizeof(double)); @@ -168,20 +172,6 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph double ek; double ekdot; double cek, sek; - double pk; - double c2pk, s2pk; - double uk; - double ukdot; - double cuk, suk; - double ok; - double sok, cok; - double ik; - double ikdot; - double sik, cik; - double rk; - double rkdot; - double xpk, ypk; - double xpkdot, ypkdot; int cnt = 0; @@ -210,8 +200,12 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph ekdot = mkdot/( 1 - eph->ecc*cek ); double tak = atan2(sqrt(1.0 - eph->ecc * eph->ecc) * sek, cek - eph->ecc); - double takdot = sek * ekdot * (1.0 + eph->ecc * cos(tak)) / (sin(tak) * (1.0 - eph->ecc* cek)); - relativistic = -4.442807633E-10 * eph->ecc * eph->sqrta * sek; + double takdot = + sqrt(1.0 - eph->ecc * eph->ecc) * ekdot / + (1.0 - eph->ecc * cek); + const double relativistic_coefficient = -4.442807633E-10; + relativistic = + relativistic_coefficient * eph->ecc * eph->sqrta * sek; double phi = tak + eph->omg0; phi = remainder(phi, 2 * PI); @@ -219,18 +213,59 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph double u = phi + eph->cus * sin(2.0 * phi) + eph->cuc * cos(2.0 * phi); double r = eph->A * (1.0 - eph->ecc * cos(ek)) + eph->crc * cos(2 * phi) + eph->crs * sin(2 * phi); double i = eph->inc0 + eph->idot * tk + eph->cic * cos(2 * phi) + eph->cis * sin(2 * phi); + double udot = takdot + + 2.0 * (eph->cus * cos(2.0 * phi) - + eph->cuc * sin(2.0 * phi)) * takdot; + double rdot = eph->A * eph->ecc * sin(ek) * ekdot + + 2.0 * (eph->crs * cos(2.0 * phi) - + eph->crc * sin(2.0 * phi)) * takdot; + double idot = eph->idot + + 2.0 * (eph->cis * cos(2.0 * phi) - + eph->cic * sin(2.0 * phi)) * takdot; double Omega = eph->omgdot + (eph->omgkdot - OMEGA_EARTH) * tk - OMEGA_EARTH * eph->toe.sec; Omega = remainder(Omega + 2 * PI, 2 * PI); + double Omega_dot = eph->omgkdot - OMEGA_EARTH; - pos[0] = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); - pos[1] = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); - pos[2] = sin(u) * r * sin(i); + double x_orbit = r * cos(u); + double y_orbit = r * sin(u); + double x_orbit_dot = rdot * cos(u) - y_orbit * udot; + double y_orbit_dot = rdot * sin(u) + x_orbit * udot; + + pos[0] = x_orbit * cos(Omega) - y_orbit * cos(i) * sin(Omega); + pos[1] = x_orbit * sin(Omega) + y_orbit * cos(i) * cos(Omega); + pos[2] = y_orbit * sin(i); + vel[0] = + x_orbit_dot * cos(Omega) - + x_orbit * sin(Omega) * Omega_dot - + y_orbit_dot * cos(i) * sin(Omega) + + y_orbit * sin(i) * idot * sin(Omega) - + y_orbit * cos(i) * cos(Omega) * Omega_dot; + vel[1] = + x_orbit_dot * sin(Omega) + + x_orbit * cos(Omega) * Omega_dot + + y_orbit_dot * cos(i) * cos(Omega) - + y_orbit * sin(i) * idot * cos(Omega) - + y_orbit * cos(i) * sin(Omega) * Omega_dot; + vel[2] = y_orbit_dot * sin(i) + y_orbit * cos(i) * idot; // Satellite clock correction // relativistic correction //*clockCorr = eph->af0 + tk * (eph->af1 + tk * eph->af2) + relativistic - eph->tgd; //7.23706041149e-05 - *clockCorr = (eph->af2 * clock_tk + eph->af1) * clock_tk + eph->af0 - eph->tgd + relativistic; + double clock_correction = + (eph->af2 * clock_tk + eph->af1) * clock_tk + + eph->af0 - eph->tgd + relativistic; + double clock_drift = + (2.0 * eph->af2 * clock_tk) + eph->af1 + + relativistic_coefficient * eph->ecc * eph->sqrta * + cek * ekdot; + if(clockCorr != NULL) { + *clockCorr = clock_correction; + } + if(clk != NULL) { + clk[0] = clock_correction; + clk[1] = clock_drift; + } /*if (clk[0] != SECONDS_IN_HALF_WEEK) { *clockCorr = clk[0];//eph->af0 + tk * (eph->af1 + tk * eph->af2) + relativistic - eph->tgd; } @@ -268,7 +303,9 @@ static bool least_squares_ura_sigma_m(unsigned char ura_index, double* sigma_m) static double least_squares_measurement_weight(unsigned char ura_index, double elevation_deg, - bool elevation_valid) + bool elevation_valid, + double cn0_dbhz, + bool cn0_valid) { const double dtr = 0.01745329251994329576923690768489; const double min_elevation_deg = 10.0; @@ -296,6 +333,25 @@ static double least_squares_measurement_weight(unsigned char ura_index, double elevation_sigma_m = elevation_sigma_zenith_m / sin_el; variance_m2 += elevation_sigma_m * elevation_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. + */ + double cn0_sigma_m = + 3.0 * pow(10.0, (45.0 - cn0_dbhz) / 20.0); + if(cn0_sigma_m < 0.5) + { + cn0_sigma_m = 0.5; + } + if(cn0_sigma_m > 30.0) + { + cn0_sigma_m = 30.0; + } + variance_m2 += cn0_sigma_m * cn0_sigma_m; + } return 1.0 / variance_m2; } @@ -527,6 +583,7 @@ static bool least_squares_build_postfit(const mat_type* satpos, const mat_type* obs, int satellite_count, const unsigned char* ura_indices, + const double* cn0_dbhz, const ionoutc_t* ionoutc, double gps_tow, const double state[4], @@ -583,7 +640,10 @@ static bool least_squares_build_postfit(const mat_type* satpos, az->matrix[0][i] = topocentric.azim; el->matrix[0][i] = topocentric.elv; - double weight = least_squares_measurement_weight(ura_indices[i], topocentric.elv, true); + double weight = least_squares_measurement_weight( + ura_indices[i], topocentric.elv, true, + (cn0_dbhz != NULL) ? cn0_dbhz[i] : NAN, + (cn0_dbhz != NULL) && isfinite(cn0_dbhz[i])); if(!isfinite(weight) || (weight <= 0.0)) { return false; @@ -629,6 +689,7 @@ static double gps_wls_last_solution[4] = {0.0, 0.0, 0.0, 0.0}; static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber, mat_type * az, mat_type * el, const unsigned char* ura_indices, + const double* cn0_dbhz, const ionoutc_t* ionoutc, double gps_tow, const double seed[4], bool seed_valid, gps_nav_solution_t* solution_out) @@ -748,7 +809,10 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS initialize_all(0.0, A); for(int i = 0 ; i < ActiveSatellitesNumber ; i++) { - absoluteWeights[i] = least_squares_measurement_weight(ura_indices[i], 0.0, false); + absoluteWeights[i] = least_squares_measurement_weight( + ura_indices[i], 0.0, false, + (cn0_dbhz != NULL) ? cn0_dbhz[i] : NAN, + (cn0_dbhz != NULL) && isfinite(cn0_dbhz[i])); } bool seeded_from_last_solution = seed_valid && (seed != NULL); @@ -843,7 +907,10 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS //TODO: topocent e giren matrix lerin elemanları doğru yükleniyor mu diye bakılacak. !!!!!!!!!!!!!!!!!1 az->matrix[0][i] = topcent->azim; el->matrix[0][i] = topcent->elv; - absoluteWeights[i] = least_squares_measurement_weight(ura_indices[i], topcent->elv, true); + absoluteWeights[i] = least_squares_measurement_weight( + ura_indices[i], topcent->elv, true, + (cn0_dbhz != NULL) ? cn0_dbhz[i] : NAN, + (cn0_dbhz != NULL) && isfinite(cn0_dbhz[i])); iono = klobuchar_delay_meters(ionoutc, gps_tow, phi_lambda_h->phi, phi_lambda_h->lambda, topcent->azim, topcent->elv); @@ -989,7 +1056,8 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS double residuals_m[GPS_TRCK_IP_COUNT]; if(!least_squares_build_postfit(satpos, obs, ActiveSatellitesNumber, - ura_indices, ionoutc, gps_tow, + ura_indices, cn0_dbhz, + ionoutc, gps_tow, posVec->matrix[0], A, az, el, residuals_m, absoluteWeights)) { @@ -1234,6 +1302,7 @@ least_squares_fail: bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber, mat_type * az, mat_type * el, const unsigned char* ura_indices, + const double* cn0_dbhz, const ionoutc_t* ionoutc, double gps_tow, gps_nav_solution_t* solution_out) { @@ -1246,7 +1315,8 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber gps_nav_solution_t full_solution; if(!least_squares_candidate(satpos, obs, ActiveSatellitesNumber, - az, el, ura_indices, ionoutc, gps_tow, + az, el, ura_indices, cn0_dbhz, + ionoutc, gps_tow, gps_wls_last_solution, gps_wls_last_solution_valid, &full_solution)) @@ -1278,6 +1348,7 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber mat_type* subset_az = create_matrix(1, subset_count, "fdeAz"); mat_type* subset_el = create_matrix(1, subset_count, "fdeEl"); unsigned char subset_ura[GPS_TRCK_IP_COUNT]; + double subset_cn0[GPS_TRCK_IP_COUNT]; if((subset_satpos == NULL) || (subset_obs == NULL) || (subset_az == NULL) || (subset_el == NULL)) { @@ -1307,6 +1378,9 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber } subset_obs->matrix[0][subset_row] = obs->matrix[0][original_row]; subset_ura[subset_row] = ura_indices[original_row]; + subset_cn0[subset_row] = + (cn0_dbhz != NULL) ? + cn0_dbhz[original_row] : NAN; subset_row++; } @@ -1317,6 +1391,7 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber gps_nav_solution_t candidate; bool candidate_ok = least_squares_candidate(subset_satpos, subset_obs, subset_count, subset_az, subset_el, subset_ura, + subset_cn0, ionoutc, gps_tow, full_seed, true, &candidate); free_matrix(subset_satpos); 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 ea0f13e..60e131d 100644 --- a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c +++ b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c @@ -58,6 +58,8 @@ static volatile bool acq_completion_queued[GPS_ACQ_IP_COUNT]; static volatile bool acq_shadow_pending[GPS_ACQ_IP_COUNT]; static gps_acq_result_t acq_shadow_result[GPS_ACQ_IP_COUNT]; static gps_tracking_shadow_t trk_shadow[GPS_TRCK_IP_COUNT]; +static gps_tracking_measurement_t + gps_latest_tracking_measurement[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]; @@ -113,6 +115,67 @@ static double gps_correct_abs_sample(uint32_t abs_sample, uint32_t rem_code_phas return (double)abs_sample - ((double)rem_code_phase / CODE_FREQ_BASIS); } +static void gps_update_tracking_measurement_isr( + volatile const gps_trck_t* tracking_ip, + unsigned char tracking_index, + uint64_t local_timestamp_ticks) +{ + gps_tracking_measurement_t measurement; + if(!gps_tracking_measurement_read(tracking_ip, &measurement)) + { + gps_runtime_diagnostics.measurement_invalid_total++; + return; + } + + gps_tracking_measurement_t* previous = + &gps_latest_tracking_measurement[tracking_index]; + if(previous->valid && (previous->prn == measurement.prn)) + { + uint16_t sequence_delta = + (uint16_t)(measurement.sequence - previous->sequence); + if(sequence_delta == 0u) + { + /* A repeated IRQ for the same mailbox does not create a sample. */ + return; + } + if(sequence_delta > 1u) + { + gps_runtime_diagnostics.measurement_missed_total += + (uint32_t)sequence_delta - 1u; + } + /* Smooth the noisy one-bit NWPR estimate; keep raw sums for diagnostics. */ + measurement.cn0_dbhz = + 0.2 * measurement.cn0_dbhz + + 0.8 * previous->cn0_dbhz; + } + measurement.local_timestamp_ticks = local_timestamp_ticks; + gps_latest_tracking_measurement[tracking_index] = measurement; +} + +static bool gps_copy_latest_tracking_measurement( + unsigned char tracking_index, + uint32_t expected_prn, + gps_tracking_measurement_t* measurement_out) +{ + if((tracking_index >= GPS_TRCK_IP_COUNT) || + (measurement_out == NULL)) + { + return false; + } + + uint64_t irq_state = bsp_irq_save(); + *measurement_out = + gps_latest_tracking_measurement[tracking_index]; + bsp_irq_restore(irq_state); + if(!measurement_out->valid || + (measurement_out->prn != expected_prn)) + { + memset(measurement_out, 0, sizeof(*measurement_out)); + return false; + } + return true; +} + static uint32_t gps_nav_ready_channel_mask(void) { uint32_t ready_mask = 0u; @@ -318,6 +381,8 @@ static void gps_clear_tracking_slot(unsigned char trck_ip_no) memset((void*)&gps_frames_parsed[trck_ip_no], 0, sizeof(gps_frames_parsed[trck_ip_no])); memset((void*)&gps_subframes[trck_ip_no][0], 0, sizeof(gps_subframes[trck_ip_no])); + memset(&gps_latest_tracking_measurement[trck_ip_no], 0, + sizeof(gps_latest_tracking_measurement[trck_ip_no])); } static void gps_set_tracking_nav_bits_mode(unsigned char trck_ip_no) @@ -332,6 +397,8 @@ static void gps_set_tracking_nav_bits_mode(unsigned char trck_ip_no) trck_enable_nav_solve(trck_ips[trck_ip_no]); trck_enable_nav_bits_flag(trck_ips[trck_ip_no]); trck_enable_nav_bits_intr(trck_ips[trck_ip_no]); + trck_enable_measurement_flag(trck_ips[trck_ip_no]); + trck_enable_measurement_intr(trck_ips[trck_ip_no]); } static void gps_runtime_state_init(void) @@ -776,6 +843,22 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip, { return; } + if((status_reg & (1u << GPS_TRCK_MEASUREMENT_FLAG_POS)) != 0u) + { + gps_update_tracking_measurement_isr( + trck_ip, trck_ip_no, local_timestamp_ticks); + } + + const uint32_t queued_source_mask = + (1u << GPS_TRCK_STOP_UNLOCK_POS) | + (1u << GPS_TRCK_NAV_BITS_RCV_POS) | + (1u << GPS_TRCK_NAV_SOLVE_FLAG_POS); + if((status_reg & queued_source_mask) == 0u) + { + /* 50 Hz channel measurements are newest-only mailboxes, not queue events. */ + trck_clear_intr_flag(trck_ip); + return; + } gps_isr_event_t event; memset(&event, 0, sizeof(event)); @@ -1376,6 +1459,10 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event) event->pps_capture_sequence; gps_nav_active_snapshot.local_timestamp_ticks[trck_ip_no] = event->local_timestamp_ticks; + (void)gps_copy_latest_tracking_measurement( + trck_ip_no, + event->prn, + &gps_nav_active_snapshot.measurement[trck_ip_no]); } if(assembly_result == GPS_NAV_EPOCH_EVENT_COMPLETE) diff --git a/soc/software/gps_system/src/gps/gps_ip.c b/soc/software/gps_system/src/gps/gps_ip.c index 7be9a29..fdf3175 100644 --- a/soc/software/gps_system/src/gps/gps_ip.c +++ b/soc/software/gps_system/src/gps/gps_ip.c @@ -660,7 +660,8 @@ static unsigned int gps_collect_completed_epoch_locked( uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT], uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT], uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT], - uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT]) + uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT], + gps_tracking_measurement_t snapshot_measurements[GPS_TRCK_IP_COUNT]) { if(epoch == NULL) { @@ -695,6 +696,8 @@ static unsigned int gps_collect_completed_epoch_locked( epoch->local_timestamp_ticks[ip_no]; snapshot_raw_samples[count] = epoch->raw_sample[ip_no]; + snapshot_measurements[count] = + epoch->measurement[ip_no]; count++; } return count; @@ -707,6 +710,7 @@ static unsigned int gps_select_coherent_nav_snapshot( uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT], uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT], uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT], + gps_tracking_measurement_t snapshot_measurements[GPS_TRCK_IP_COUNT], unsigned int candidate_count) { if((candidate_count == 0u) || @@ -756,6 +760,8 @@ static unsigned int gps_select_coherent_nav_snapshot( snapshot_local_timestamps[source]; snapshot_raw_samples[destination] = snapshot_raw_samples[source]; + snapshot_measurements[destination] = + snapshot_measurements[source]; } snapshot[destination].abs_sample = gps_unwrap_corrected_abs_sample( @@ -794,10 +800,12 @@ static uint64_t gps_nav_median_local_timestamp( static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, const uint64_t pps_counters[GPS_TRCK_IP_COUNT], const uint32_t pps_capture_sequences[GPS_TRCK_IP_COUNT], + const gps_tracking_measurement_t* measurements, unsigned int satellite_count, gps_nav_solution_t* solution_out) { - if((ephemeris == NULL) || (solution_out == NULL) || + if((ephemeris == NULL) || (measurements == NULL) || + (solution_out == NULL) || (satellite_count < GPS_NAV_MIN_WLS_SATELLITES)) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "nav solve skipped, usable satellites=%u\r\n", satellite_count); @@ -809,7 +817,9 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, range_t* rho0 = (range_t*)malloc(satellite_count * sizeof(range_t)); double* psTVLtime = (double*)malloc(satellite_count * sizeof(double)); double* clockCorr = (double*)malloc(satellite_count * sizeof(double)); + double* clockDrift = (double*)malloc(satellite_count * sizeof(double)); unsigned char ura_indices[GPS_TRCK_IP_COUNT]; + double cn0_dbhz[GPS_TRCK_IP_COUNT]; mat_type* satellitePositions = create_matrix(satellite_count, 3, "satellitePositions"); mat_type* velocities = create_matrix(satellite_count, 3, "velocities"); mat_type* preciseTxTimes = NULL; @@ -825,7 +835,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "Klobuchar correction %s for this solve\r\n", (ionoutc != NULL) ? "available" : "waiting for subframe 4 page 18"); - if((rho0 == NULL) || (psTVLtime == NULL) || (clockCorr == NULL) || + if((rho0 == NULL) || (psTVLtime == NULL) || + (clockCorr == NULL) || (clockDrift == NULL) || (satellitePositions == NULL) || (velocities == NULL)) { GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_MEM, "nav solve allocation failed\r\n"); @@ -835,6 +846,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, for(unsigned int i = 0 ; i < satellite_count ; i++) { ura_indices[i] = ephemeris[i].sva; + cn0_dbhz[i] = + (measurements[i].valid && + (measurements[i].prn == ephemeris[i].SVID)) + ? measurements[i].cn0_dbhz : NAN; psTVLtime[i] = DBL_MAX; } @@ -847,7 +862,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, "snapshot[%u]: svid=%u abs=%.6f tow=%.3f\r\n", i, ephemeris[i].SVID, ephemeris[i].abs_sample, ephemeris[i].tow.sec); computeRange(&(psTVLtime[i]), i, rho0, (gps_ephemeris_t*)&ephemeris[i], - NULL, NULL, NULL, &clockCorr[i], satellitePositions, velocities); + NULL, NULL, NULL, &clockCorr[i], &clockDrift[i], + satellitePositions, velocities); } double minimum = psTVLtime[0]; @@ -891,8 +907,57 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, } ok = leastSquaresSTD(satellitePositions, localPseudoRanges, satellite_count, az, el, - ura_indices, + ura_indices, cn0_dbhz, ionoutc, ephemeris[0].tow.sec, &solution); + for(unsigned int row = 0u; row < satellite_count; row++) + { + solution.cn0_dbhz[row] = measurements[row].cn0_dbhz; + solution.doppler_hz[row] = measurements[row].doppler_hz; + solution.pseudorange_rate_mps[row] = + measurements[row].pseudorange_rate_mps; + } + if(ok && solution.solution_valid) + { + double satellite_positions_array[GPS_TRCK_IP_COUNT][3]; + double satellite_velocities_array[GPS_TRCK_IP_COUNT][3]; + gps_tracking_measurement_t + velocity_measurements[GPS_TRCK_IP_COUNT]; + for(unsigned int row = 0u; row < satellite_count; row++) + { + for(unsigned int component = 0u; component < 3u; component++) + { + satellite_positions_array[row][component] = + satellitePositions->matrix[row][component]; + satellite_velocities_array[row][component] = + velocities->matrix[row][component]; + } + velocity_measurements[row] = measurements[row]; + if((solution.used_row_mask & (1u << row)) == 0u) + { + velocity_measurements[row].valid = false; + } + } + solution.velocity_valid = gps_tracking_velocity_wls( + solution.ecef_m, + satellite_positions_array, + satellite_velocities_array, + clockDrift, + velocity_measurements, + satellite_count, + solution.velocity_ecef_mps, + &solution.clock_drift_mps, + &solution.velocity_residual_rms_mps); + if(solution.velocity_valid) + { + solution.speed_mps = + sqrt(solution.velocity_ecef_mps[0] * + solution.velocity_ecef_mps[0] + + solution.velocity_ecef_mps[1] * + solution.velocity_ecef_mps[1] + + solution.velocity_ecef_mps[2] * + solution.velocity_ecef_mps[2]); + } + } if(ok && solution.fde_applied) { for(unsigned int row = 0 ; row < satellite_count ; row++) @@ -944,6 +1009,7 @@ cleanup: free(rho0); free(psTVLtime); free(clockCorr); + free(clockDrift); *solution_out = solution; return ok; } @@ -1076,6 +1142,7 @@ bool calculate_position(gps_nav_solution_t* solution_out, uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT]; uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT]; uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT]; + gps_tracking_measurement_t snapshot_measurements[GPS_TRCK_IP_COUNT]; gps_nav_epoch_snapshot_t completed_epoch; unsigned int satellite_count = 0; unsigned int candidate_count = 0; @@ -1119,7 +1186,8 @@ bool calculate_position(gps_nav_solution_t* solution_out, &completed_epoch, snapshot, snapshot_ips, snapshot_pps_counters, snapshot_pps_sequences, - snapshot_local_timestamps, snapshot_raw_samples); + snapshot_local_timestamps, snapshot_raw_samples, + snapshot_measurements); unsigned int completed_channel_count = gps_nav_mask_count(completed_epoch.channel_mask); if(candidate_count != completed_channel_count) @@ -1144,6 +1212,7 @@ bool calculate_position(gps_nav_solution_t* solution_out, snapshot, snapshot_ips, snapshot_pps_counters, snapshot_pps_sequences, snapshot_local_timestamps, snapshot_raw_samples, + snapshot_measurements, candidate_count); coherent_nav_count = satellite_count; run_nav = satellite_count >= GPS_NAV_MIN_WLS_SATELLITES; @@ -1197,7 +1266,9 @@ bool calculate_position(gps_nav_solution_t* solution_out, memset(&nav_solution, 0, sizeof(nav_solution)); uint64_t nav_software_start_cycles = gps_read_cycle_counter(); bool nav_ok = nav_software_from_snapshot(snapshot, snapshot_pps_counters, - snapshot_pps_sequences, satellite_count, + snapshot_pps_sequences, + snapshot_measurements, + satellite_count, &nav_solution); uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles; if(nav_ok) @@ -1371,6 +1442,26 @@ void trck_enable_nav_bits_flag(volatile gps_trck_t* trck_ip) trck_ip->CONTROL_REG |= (1 << GPS_TRCK_NAV_BITS_FLAG_EN_POS); } +void trck_enable_measurement_intr(volatile gps_trck_t* trck_ip) +{ + trck_ip->CONTROL_REG |= (1u << GPS_TRCK_MEASUREMENT_INTR_EN_POS); +} + +void trck_disable_measurement_intr(volatile gps_trck_t* trck_ip) +{ + trck_ip->CONTROL_REG &= ~(1u << GPS_TRCK_MEASUREMENT_INTR_EN_POS); +} + +void trck_enable_measurement_flag(volatile gps_trck_t* trck_ip) +{ + trck_ip->CONTROL_REG |= (1u << GPS_TRCK_MEASUREMENT_FLAG_EN_POS); +} + +void trck_disable_measurement_flag(volatile gps_trck_t* trck_ip) +{ + trck_ip->CONTROL_REG &= ~(1u << GPS_TRCK_MEASUREMENT_FLAG_EN_POS); +} + void trck_set_nav_solve_timer(unsigned int tick, volatile gps_trck_t* trck_ip) { @@ -1421,6 +1512,8 @@ void gps_startup_initialization(unsigned int nsp_tick) trck_enable_nav_bits_flag(trck_ips[i]); trck_enable_nav_bits_intr(trck_ips[i]); + trck_enable_measurement_flag(trck_ips[i]); + trck_enable_measurement_intr(trck_ips[i]); // clear all status flags. trck_set_nav_solve_timer(nsp_tick, trck_ips[i]); diff --git a/soc/software/gps_system/src/gps/gps_tracking_measurement.c b/soc/software/gps_system/src/gps/gps_tracking_measurement.c new file mode 100644 index 0000000..dde57fe --- /dev/null +++ b/soc/software/gps_system/src/gps/gps_tracking_measurement.c @@ -0,0 +1,355 @@ +#include "gps/gps_tracking_measurement.h" + +#include +#include +#include + +#define GPS_TRACK_MEASUREMENT_READ_ATTEMPTS 3u +#define GPS_TRACK_MEASUREMENT_MIN_SATELLITES 4u +#define GPS_TRACK_MEASUREMENT_MAX_SATELLITES GPS_TRCK_IP_COUNT +#define GPS_TRACK_MEASUREMENT_EPSILON 1.0e-12 + +static double clamp_double(double value, double minimum, double maximum) +{ + if(value < minimum) + { + return minimum; + } + if(value > maximum) + { + return maximum; + } + return value; +} + +bool gps_tracking_cn0_dbhz( + int32_t sum_i, + int32_t sum_q, + uint64_t wideband_power, + unsigned int coherent_samples, + double* cn0_dbhz_out) +{ + if((cn0_dbhz_out == NULL) || (wideband_power == 0u) || + (coherent_samples < 2u)) + { + return false; + } + + double i = (double)sum_i; + double q = (double)sum_q; + double narrowband_power = i * i + q * q; + double narrow_to_wide_ratio = + narrowband_power / (double)wideband_power; + double sample_count = (double)coherent_samples; + + /* + * Narrowband/wideband-power estimator with 1 ms coherent prompt + * integrations: C/N0 = (1/T) * (R - 1) / (M - R). + */ + if(!isfinite(narrow_to_wide_ratio) || + (narrow_to_wide_ratio <= 1.0) || + (narrow_to_wide_ratio >= sample_count)) + { + return false; + } + double cn0_linear = + 1000.0 * (narrow_to_wide_ratio - 1.0) / + (sample_count - narrow_to_wide_ratio); + if(!isfinite(cn0_linear) || (cn0_linear <= 0.0)) + { + return false; + } + + double cn0_dbhz = 10.0 * log10(cn0_linear); + if(!isfinite(cn0_dbhz)) + { + return false; + } + *cn0_dbhz_out = clamp_double(cn0_dbhz, 10.0, 60.0); + return true; +} + +double gps_tracking_doppler_hz(int32_t carrier_freq_q7) +{ + return ((double)carrier_freq_q7 / GPS_TRCK_CARRIER_Q7_SCALE) - + GPS_TRCK_IF_CENTER_HZ; +} + +double gps_tracking_doppler_to_range_rate_mps(double doppler_hz) +{ + const double wavelength_m = + (double)SPEED_OF_LIGHT / GPS_L1_CARRIER_HZ; + /* Positive received-frequency Doppler means decreasing geometric range. */ + return -doppler_hz * wavelength_m; +} + +bool gps_tracking_measurement_read( + volatile const gps_trck_t* tracking_ip, + gps_tracking_measurement_t* measurement_out) +{ + if((tracking_ip == NULL) || (measurement_out == NULL)) + { + return false; + } + + for(unsigned int attempt = 0u; + attempt < GPS_TRACK_MEASUREMENT_READ_ATTEMPTS; + attempt++) + { + uint32_t sequence_before = tracking_ip->MEASUREMENT_SEQUENCE; + uint32_t meta = tracking_ip->MEASUREMENT_META; + int32_t carrier_freq_q7 = tracking_ip->MEASUREMENT_CARRIER_Q7; + uint32_t absolute_sample = tracking_ip->MEASUREMENT_ABS_SAMPLE; + uint32_t remaining_code_phase = + tracking_ip->MEASUREMENT_REM_CODE_PHASE; + int32_t sum_i = tracking_ip->MEASUREMENT_SUM_I; + int32_t sum_q = tracking_ip->MEASUREMENT_SUM_Q; + uint32_t wbp_low = tracking_ip->MEASUREMENT_WBP_LO; + uint32_t wbp_high = tracking_ip->MEASUREMENT_WBP_HI; + uint32_t sequence_after = tracking_ip->MEASUREMENT_SEQUENCE; + + if(sequence_before != sequence_after) + { + continue; + } + + memset(measurement_out, 0, sizeof(*measurement_out)); + measurement_out->prn = tracking_ip->PRN_NUMBER; + measurement_out->sequence = sequence_after & 0xffffu; + measurement_out->absolute_sample = absolute_sample; + measurement_out->remaining_code_phase = remaining_code_phase; + measurement_out->carrier_freq_q7 = carrier_freq_q7; + measurement_out->cn0_sum_i = sum_i; + measurement_out->cn0_sum_q = sum_q; + measurement_out->cn0_wbp = + ((uint64_t)(wbp_high & 0x3ffffu) << 32) | wbp_low; + measurement_out->cn0_sample_count = + (meta >> GPS_TRCK_MEAS_META_COUNT_SHIFT) & + GPS_TRCK_MEAS_META_COUNT_MASK; + measurement_out->nav_bit = + (meta & (1u << GPS_TRCK_MEAS_META_NAV_BIT_POS)) != 0u; + measurement_out->locked = + (meta & (1u << GPS_TRCK_MEAS_META_LOCK_POS)) != 0u; + measurement_out->doppler_hz = + gps_tracking_doppler_hz(carrier_freq_q7); + measurement_out->pseudorange_rate_mps = + gps_tracking_doppler_to_range_rate_mps( + measurement_out->doppler_hz); + + unsigned int version = + meta >> GPS_TRCK_MEAS_META_VERSION_SHIFT; + bool cn0_valid = gps_tracking_cn0_dbhz( + sum_i, sum_q, measurement_out->cn0_wbp, + measurement_out->cn0_sample_count, + &measurement_out->cn0_dbhz); + measurement_out->valid = + (version == GPS_TRCK_MEAS_META_VERSION) && + ((meta & (1u << GPS_TRCK_MEAS_META_VALID_POS)) != 0u) && + measurement_out->locked && + (measurement_out->cn0_sample_count == 20u) && + cn0_valid && + isfinite(measurement_out->doppler_hz) && + isfinite(measurement_out->pseudorange_rate_mps); + return measurement_out->valid; + } + + memset(measurement_out, 0, sizeof(*measurement_out)); + return false; +} + +static bool solve_four_by_four( + double normal[4][4], + double rhs[4], + double solution[4]) +{ + double augmented[4][5]; + for(unsigned int row = 0u; row < 4u; row++) + { + for(unsigned int column = 0u; column < 4u; column++) + { + augmented[row][column] = normal[row][column]; + } + augmented[row][4] = rhs[row]; + } + + for(unsigned int pivot = 0u; pivot < 4u; pivot++) + { + unsigned int best_row = pivot; + double best_value = fabs(augmented[pivot][pivot]); + for(unsigned int row = pivot + 1u; row < 4u; row++) + { + double value = fabs(augmented[row][pivot]); + if(value > best_value) + { + best_value = value; + best_row = row; + } + } + if(!isfinite(best_value) || + (best_value <= GPS_TRACK_MEASUREMENT_EPSILON)) + { + return false; + } + if(best_row != pivot) + { + for(unsigned int column = pivot; column < 5u; column++) + { + double temporary = augmented[pivot][column]; + augmented[pivot][column] = + augmented[best_row][column]; + augmented[best_row][column] = temporary; + } + } + + double divisor = augmented[pivot][pivot]; + for(unsigned int column = pivot; column < 5u; column++) + { + augmented[pivot][column] /= divisor; + } + for(unsigned int row = 0u; row < 4u; row++) + { + if(row == pivot) + { + continue; + } + double factor = augmented[row][pivot]; + for(unsigned int column = pivot; column < 5u; column++) + { + augmented[row][column] -= + factor * augmented[pivot][column]; + } + } + } + + for(unsigned int row = 0u; row < 4u; row++) + { + solution[row] = augmented[row][4]; + if(!isfinite(solution[row])) + { + return false; + } + } + return true; +} + +bool gps_tracking_velocity_wls( + const double receiver_ecef_m[3], + const double satellite_positions_m[][3], + const double satellite_velocities_mps[][3], + const double satellite_clock_drifts_sps[], + const gps_tracking_measurement_t measurements[], + unsigned int count, + double receiver_velocity_ecef_mps_out[3], + double* receiver_clock_drift_mps_out, + double* residual_rms_mps_out) +{ + if((receiver_ecef_m == NULL) || (satellite_positions_m == NULL) || + (satellite_velocities_mps == NULL) || + (satellite_clock_drifts_sps == NULL) || + (measurements == NULL) || + (receiver_velocity_ecef_mps_out == NULL) || + (receiver_clock_drift_mps_out == NULL) || + (residual_rms_mps_out == NULL) || + (count < GPS_TRACK_MEASUREMENT_MIN_SATELLITES) || + (count > GPS_TRACK_MEASUREMENT_MAX_SATELLITES)) + { + return false; + } + + double normal[4][4] = {{0.0}}; + double rhs[4] = {0.0}; + double design[GPS_TRACK_MEASUREMENT_MAX_SATELLITES][4]; + double observation[GPS_TRACK_MEASUREMENT_MAX_SATELLITES]; + unsigned int used = 0u; + + for(unsigned int satellite = 0u; satellite < count; satellite++) + { + const gps_tracking_measurement_t* measurement = + &measurements[satellite]; + if(!measurement->valid || !measurement->locked || + !isfinite(measurement->cn0_dbhz) || + !isfinite(measurement->pseudorange_rate_mps)) + { + continue; + } + + double delta[3]; + double range_squared = 0.0; + for(unsigned int component = 0u; component < 3u; component++) + { + delta[component] = + satellite_positions_m[satellite][component] - + receiver_ecef_m[component]; + range_squared += delta[component] * delta[component]; + } + double range = sqrt(range_squared); + if(!isfinite(range) || (range < 1.0)) + { + continue; + } + + double satellite_los_rate = 0.0; + for(unsigned int component = 0u; component < 3u; component++) + { + double unit_los = delta[component] / range; + design[used][component] = -unit_los; + satellite_los_rate += + unit_los * + satellite_velocities_mps[satellite][component]; + } + design[used][3] = 1.0; + observation[used] = + measurement->pseudorange_rate_mps - + satellite_los_rate + + (double)SPEED_OF_LIGHT * + satellite_clock_drifts_sps[satellite]; + + /* Doppler precision improves approximately with signal amplitude. */ + double sigma_rate_mps = + 30.0 * pow(10.0, -measurement->cn0_dbhz / 20.0); + sigma_rate_mps = clamp_double(sigma_rate_mps, 0.05, 5.0); + double weight = 1.0 / (sigma_rate_mps * sigma_rate_mps); + for(unsigned int row = 0u; row < 4u; row++) + { + rhs[row] += + weight * design[used][row] * observation[used]; + for(unsigned int column = 0u; column < 4u; column++) + { + normal[row][column] += + weight * design[used][row] * + design[used][column]; + } + } + used++; + } + + if(used < GPS_TRACK_MEASUREMENT_MIN_SATELLITES) + { + return false; + } + + double solution[4]; + if(!solve_four_by_four(normal, rhs, solution)) + { + return false; + } + + double residual_square_sum = 0.0; + for(unsigned int row = 0u; row < used; row++) + { + double predicted = 0.0; + for(unsigned int column = 0u; column < 4u; column++) + { + predicted += design[row][column] * solution[column]; + } + double residual = observation[row] - predicted; + residual_square_sum += residual * residual; + } + + memcpy(receiver_velocity_ecef_mps_out, solution, + 3u * sizeof(solution[0])); + *receiver_clock_drift_mps_out = solution[3]; + *residual_rms_mps_out = + sqrt(residual_square_sum / (double)used); + return isfinite(*residual_rms_mps_out); +} diff --git a/soc/software/gps_system/tests/Makefile b/soc/software/gps_system/tests/Makefile index 183cc3b..eafc789 100644 --- a/soc/software/gps_system/tests/Makefile +++ b/soc/software/gps_system/tests/Makefile @@ -6,6 +6,8 @@ TARGETS := test_imu_calibration test_gps_nav_epoch \ test_gps_nav_epoch_coalescing test_gps_time_anchor \ test_plic_warm_reset test_gps_acq_manager \ test_gps_ephemeris_cache test_gps_receiver_hardware_sim +TARGETS += test_gps_tracking_measurement +TARGETS += test_gps_satellite_velocity .PHONY: all run sanitize clean @@ -37,6 +39,17 @@ test_gps_receiver_hardware_sim: test_gps_receiver_hardware_sim.c \ $(ROOT)/src/gps/gps_ephemeris_cache.c $(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -o $@ +test_gps_tracking_measurement: test_gps_tracking_measurement.c \ + $(ROOT)/src/gps/gps_tracking_measurement.c + $(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@ + +test_gps_satellite_velocity: test_gps_satellite_velocity.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 $@ + run: $(TARGETS) ./test_imu_calibration ./test_gps_nav_epoch @@ -46,6 +59,8 @@ run: $(TARGETS) ./test_gps_acq_manager ./test_gps_ephemeris_cache ./test_gps_receiver_hardware_sim + ./test_gps_tracking_measurement + ./test_gps_satellite_velocity sanitize: $(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \ @@ -87,9 +102,31 @@ sanitize: ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ UBSAN_OPTIONS=halt_on_error=1 \ ./test_gps_receiver_hardware_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_tracking_measurement.c \ + $(ROOT)/src/gps/gps_tracking_measurement.c -lm \ + -o test_gps_tracking_measurement_san + ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ + UBSAN_OPTIONS=halt_on_error=1 \ + ./test_gps_tracking_measurement_san + $(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \ + -fsanitize=address,undefined -fno-omit-frame-pointer \ + -Wno-comment -Wno-unused-parameter -Wno-unused-variable \ + -Wno-sign-compare -Ifake_bsp -I$(ROOT)/header \ + test_gps_satellite_velocity.c \ + $(ROOT)/src/gps/NavEquations.c \ + $(ROOT)/src/gps/matrixLib.c -lm \ + -o test_gps_satellite_velocity_san + ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ + UBSAN_OPTIONS=halt_on_error=1 \ + ./test_gps_satellite_velocity_san clean: rm -f $(TARGETS) test_gps_nav_epoch_coalescing_repro \ test_gps_nav_epoch_san test_gps_nav_epoch_coalescing_san \ test_gps_acq_manager_san test_gps_ephemeris_cache_san \ - test_gps_receiver_hardware_sim_san + test_gps_receiver_hardware_sim_san \ + test_gps_tracking_measurement_san \ + test_gps_satellite_velocity_san diff --git a/soc/software/gps_system/tests/test_gps_satellite_velocity.c b/soc/software/gps_system/tests/test_gps_satellite_velocity.c new file mode 100644 index 0000000..a30ac04 --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_satellite_velocity.c @@ -0,0 +1,149 @@ +#include "gps/NavEquations.h" + +#include +#include +#include +#include + +static void test_broadcast_ephemeris_velocity(void) +{ + gps_ephemeris_t ephemeris; + memset(&ephemeris, 0, sizeof(ephemeris)); + ephemeris.tow.week = 2400; + ephemeris.tow.sec = 345700.0; + ephemeris.toe.week = 2400; + ephemeris.toe.sec = 345600.0; + ephemeris.toc.week = 2400; + ephemeris.toc.sec = 345600.0; + ephemeris.sqrta = 5153.7954775; + ephemeris.A = ephemeris.sqrta * ephemeris.sqrta; + ephemeris.ecc = 0.0093; + ephemeris.m0 = 0.75; + ephemeris.deltan = 4.5e-9; + ephemeris.cuc = 1.0e-6; + ephemeris.cus = -3.0e-6; + ephemeris.crc = 180.0; + ephemeris.crs = -75.0; + ephemeris.cic = 1.2e-7; + ephemeris.cis = -8.0e-8; + ephemeris.inc0 = 0.96; + ephemeris.idot = -2.0e-10; + /* Legacy names: omgdot=Omega0, omg0=omega, omgkdot=Omega-dot. */ + ephemeris.omgdot = 1.1; + ephemeris.omg0 = -0.4; + ephemeris.omgkdot = -8.0e-9; + ephemeris.af0 = 2.3e-5; + ephemeris.af1 = -4.0e-12; + ephemeris.af2 = 1.0e-19; + ephemeris.tgd = -2.0e-9; + + double position[3]; + double velocity[3]; + double clock[2]; + double correction; + satpos(&ephemeris, position, velocity, clock, &correction); + + const double dt = 0.05; + gps_ephemeris_t before = ephemeris; + gps_ephemeris_t after = ephemeris; + before.tow.sec -= dt; + after.tow.sec += dt; + double position_before[3], position_after[3]; + double velocity_unused[3], clock_before[2], clock_after[2]; + double correction_before, correction_after; + satpos(&before, position_before, velocity_unused, + clock_before, &correction_before); + satpos(&after, position_after, velocity_unused, + clock_after, &correction_after); + + double radius_squared = 0.0; + double speed_squared = 0.0; + for(unsigned int component = 0u; component < 3u; component++) + { + double finite_difference = + (position_after[component] - position_before[component]) / + (2.0 * dt); + assert(fabs(velocity[component] - finite_difference) < 0.05); + radius_squared += position[component] * position[component]; + speed_squared += velocity[component] * velocity[component]; + } + double finite_clock_drift = + (correction_after - correction_before) / (2.0 * dt); + assert(fabs(clock[1] - finite_clock_drift) < 1.0e-13); + assert(fabs(clock[0] - correction) < 1.0e-18); + assert(sqrt(radius_squared) > 25000000.0); + assert(sqrt(radius_squared) < 28000000.0); + assert(sqrt(speed_squared) > 1000.0); + assert(sqrt(speed_squared) < 5000.0); +} + +static void test_cn0_changes_position_weight(void) +{ + const unsigned int satellite_count = 6u; + const double receiver[3] = {6378137.0, 0.0, 0.0}; + const double directions[6][3] = { + { 1.0, 0.0, 0.0}, + { 0.2, 0.9, 0.3872983346207417}, + { 0.3, -0.4, 0.8660254037844386}, + {-0.2, 0.5, 0.8426149773176358}, + { 0.4, -0.8, -0.4472135954999579}, + {-0.5, -0.5, 0.7071067811865475} + }; + mat_type* satellite_positions = + create_matrix((int)satellite_count, 3, "testSatPos"); + mat_type* observations = + create_matrix(1, (int)satellite_count, "testObs"); + mat_type* az = create_matrix(1, (int)satellite_count, "testAz"); + mat_type* el = create_matrix(1, (int)satellite_count, "testEl"); + assert(satellite_positions != NULL); + assert(observations != NULL); + assert(az != NULL); + assert(el != NULL); + initialize_all(0.0, satellite_positions); + initialize_all(0.0, observations); + initialize_all(0.0, az); + initialize_all(0.0, el); + + unsigned char ura[6] = {0u, 0u, 0u, 0u, 0u, 0u}; + double cn0_dbhz[6] = {52.0, 22.0, 45.0, 45.0, 45.0, 45.0}; + for(unsigned int satellite = 0u; + satellite < satellite_count; + satellite++) + { + double range_squared = 0.0; + for(unsigned int component = 0u; component < 3u; component++) + { + satellite_positions->matrix[satellite][component] = + receiver[component] + + directions[satellite][component] * 21000000.0; + double difference = + satellite_positions->matrix[satellite][component] - + receiver[component]; + range_squared += difference * difference; + } + observations->matrix[0][satellite] = + sqrt(range_squared) + 125.0; + } + + gps_nav_solution_t solution; + assert(leastSquaresSTD( + satellite_positions, observations, (int)satellite_count, + az, el, ura, cn0_dbhz, NULL, 345700.0, &solution)); + assert(isfinite(solution.measurement_sigma_m[0])); + assert(isfinite(solution.measurement_sigma_m[1])); + assert(solution.measurement_sigma_m[1] > + solution.measurement_sigma_m[0] + 20.0); + + free_matrix(satellite_positions); + free_matrix(observations); + free_matrix(az); + free_matrix(el); +} + +int main(void) +{ + test_broadcast_ephemeris_velocity(); + test_cn0_changes_position_weight(); + puts("test_gps_satellite_velocity PASS"); + return 0; +} diff --git a/soc/software/gps_system/tests/test_gps_tracking_measurement.c b/soc/software/gps_system/tests/test_gps_tracking_measurement.c new file mode 100644 index 0000000..dc7739c --- /dev/null +++ b/soc/software/gps_system/tests/test_gps_tracking_measurement.c @@ -0,0 +1,179 @@ +#include "gps/gps_tracking_measurement.h" + +#include +#include +#include +#include + +#define TEST_SATELLITES 6u + +static bool close_to(double actual, double expected, double tolerance) +{ + return fabs(actual - expected) <= tolerance; +} + +static uint64_t cn0_wbp_for_target( + int32_t sum_i, + int32_t sum_q, + unsigned int sample_count, + double cn0_dbhz) +{ + double cn0_linear = pow(10.0, cn0_dbhz / 10.0); + double scaled = cn0_linear / 1000.0; + double ratio = + (scaled * (double)sample_count + 1.0) / (scaled + 1.0); + double narrowband = + (double)sum_i * (double)sum_i + + (double)sum_q * (double)sum_q; + return (uint64_t)llround(narrowband / ratio); +} + +static void test_cn0_and_mailbox_decode(void) +{ + gps_trck_t registers; + memset(®isters, 0, sizeof(registers)); + + const int32_t sum_i = 20000; + const int32_t sum_q = 1000; + const uint64_t wbp = + cn0_wbp_for_target(sum_i, sum_q, 20u, 45.0); + const int32_t carrier_q7 = + (int32_t)llround((GPS_TRCK_IF_CENTER_HZ + 2500.0) * + GPS_TRCK_CARRIER_Q7_SCALE); + + registers.PRN_NUMBER = 7u; + registers.MEASUREMENT_META = + (GPS_TRCK_MEAS_META_VERSION << + GPS_TRCK_MEAS_META_VERSION_SHIFT) | + (20u << GPS_TRCK_MEAS_META_COUNT_SHIFT) | + (1u << GPS_TRCK_MEAS_META_VALID_POS) | + (1u << GPS_TRCK_MEAS_META_NAV_BIT_POS) | + (1u << GPS_TRCK_MEAS_META_LOCK_POS); + registers.MEASUREMENT_SEQUENCE = 0x1234u; + registers.MEASUREMENT_CARRIER_Q7 = carrier_q7; + registers.MEASUREMENT_ABS_SAMPLE = 0x89abcdefu; + registers.MEASUREMENT_REM_CODE_PHASE = 555u; + registers.MEASUREMENT_SUM_I = sum_i; + registers.MEASUREMENT_SUM_Q = sum_q; + registers.MEASUREMENT_WBP_LO = (uint32_t)wbp; + registers.MEASUREMENT_WBP_HI = (uint32_t)(wbp >> 32); + + gps_tracking_measurement_t measurement; + assert(gps_tracking_measurement_read(®isters, &measurement)); + assert(measurement.valid); + assert(measurement.locked); + assert(measurement.nav_bit); + assert(measurement.prn == 7u); + assert(measurement.sequence == 0x1234u); + assert(measurement.absolute_sample == 0x89abcdefu); + assert(measurement.remaining_code_phase == 555u); + assert(measurement.cn0_sample_count == 20u); + assert(measurement.cn0_wbp == wbp); + assert(close_to(measurement.cn0_dbhz, 45.0, 0.01)); + assert(close_to(measurement.doppler_hz, 2500.0, 1.0 / 128.0)); + + const double expected_range_rate = + -2500.0 * ((double)SPEED_OF_LIGHT / GPS_L1_CARRIER_HZ); + assert(close_to( + measurement.pseudorange_rate_mps, + expected_range_rate, + 1.0e-9)); + + registers.MEASUREMENT_META &= + ~(1u << GPS_TRCK_MEAS_META_LOCK_POS); + assert(!gps_tracking_measurement_read(®isters, &measurement)); + assert(!measurement.valid); +} + +static void test_velocity_wls(void) +{ + const double receiver_position[3] = {6378137.0, 0.0, 0.0}; + const double directions[TEST_SATELLITES][3] = { + { 1.0, 0.0, 0.0}, + { 0.0, 1.0, 0.0}, + { 0.0, 0.0, 1.0}, + {-0.5773502691896258, 0.5773502691896258, 0.5773502691896258}, + { 0.7071067811865475, -0.7071067811865475, 0.0}, + { 0.4082482904638631, 0.4082482904638631, -0.8164965809277261} + }; + const double satellite_velocities[TEST_SATELLITES][3] = { + { 0.0, 2700.0, 100.0}, + {-900.0, 0.0, 2500.0}, + {1200.0, -2100.0, 0.0}, + {1800.0, 400.0, -800.0}, + {-500.0, 1900.0, 600.0}, + { 700.0, -1200.0, 2200.0} + }; + const double satellite_clock_drift[TEST_SATELLITES] = { + 1.0e-11, -2.0e-11, 0.5e-11, 1.5e-11, -0.7e-11, 2.2e-11 + }; + const double expected_velocity[3] = {12.0, -4.0, 1.5}; + const double expected_clock_drift_mps = 18.0; + + double satellite_positions[TEST_SATELLITES][3]; + gps_tracking_measurement_t measurements[TEST_SATELLITES]; + memset(measurements, 0, sizeof(measurements)); + for(unsigned int satellite = 0u; + satellite < TEST_SATELLITES; + satellite++) + { + double los_satellite_velocity = 0.0; + double los_receiver_velocity = 0.0; + for(unsigned int component = 0u; component < 3u; component++) + { + satellite_positions[satellite][component] = + receiver_position[component] + + directions[satellite][component] * 21000000.0; + los_satellite_velocity += + directions[satellite][component] * + satellite_velocities[satellite][component]; + los_receiver_velocity += + directions[satellite][component] * + expected_velocity[component]; + } + measurements[satellite].valid = true; + measurements[satellite].locked = true; + measurements[satellite].cn0_dbhz = 42.0 + (double)satellite; + measurements[satellite].pseudorange_rate_mps = + los_satellite_velocity - + los_receiver_velocity + + expected_clock_drift_mps - + (double)SPEED_OF_LIGHT * + satellite_clock_drift[satellite]; + } + + double solved_velocity[3]; + double solved_clock_drift; + double residual_rms; + assert(gps_tracking_velocity_wls( + receiver_position, + satellite_positions, + satellite_velocities, + satellite_clock_drift, + measurements, + TEST_SATELLITES, + solved_velocity, + &solved_clock_drift, + &residual_rms)); + + for(unsigned int component = 0u; component < 3u; component++) + { + assert(close_to( + solved_velocity[component], + expected_velocity[component], + 1.0e-9)); + } + assert(close_to( + solved_clock_drift, + expected_clock_drift_mps, + 1.0e-9)); + assert(residual_rms < 1.0e-9); +} + +int main(void) +{ + test_cn0_and_mailbox_decode(); + test_velocity_wls(); + puts("test_gps_tracking_measurement PASS"); + return 0; +}