feat(gps): expose Doppler and signal quality measurements

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),
+						 .trk_rstn(trck_resetn),
-						 .afe_rstn(PRESETN),
+						 .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
+						 .abs_sample_o(abs_sample), // 20 ms bit valid aligned
-							 .rem_code_ph_o(rem_code_ph),
+						 .rem_code_ph_o(rem_code_ph),
-							 .lock_o(lock),
+						 .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)              |
+							(busy_syned              << TRCK_BUSY_POS)              |
-								 		(lock_syned              << TRCK_LOCKED_POS)            |
+							(lock_syned              << TRCK_LOCKED_POS)            |
-								 		(0                       << TRCK_FIFO_OV_POS)           | // fifo overflow is not considered for now.
+							(0                       << TRCK_FIFO_OV_POS)           | // fifo overflow is not considered for now.
-								 		(intr_pending_p          << TRCK_INTR_PEND_POS)         |
+							(intr_pending_p          << TRCK_INTR_PEND_POS)         |
-								 		(trck_timeout_flag_p     << TRCK_UNLOCK_FLAG_POS)       |
+							(trck_timeout_flag_p     << TRCK_UNLOCK_FLAG_POS)       |
-								 		(trck_nav_bit_rcv_flag_p << TRCK_NAV_BITS_RCV_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_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

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;

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)

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

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),

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)

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

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

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

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

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

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

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

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);

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);

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.
 

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 <stdbool.h>
+#include <stdint.h>
+
+#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

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;
 }

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(

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));
+     double takdot =
-     relativistic = -4.442807633E-10 * eph->ecc * eph->sqrta * sek;
+         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);
+     double x_orbit = r * cos(u);
-     pos[1] = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega);
+     double y_orbit = r * sin(u);
-     pos[2] = sin(u) * r * sin(i);
+     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);

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)

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]);

soc/software/gps_system/src/gps/gps_tracking_measurement.c

@@ -0,0 +1,355 @@
+#include "gps/gps_tracking_measurement.h"
+
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+#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);
+}

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

soc/software/gps_system/tests/test_gps_satellite_velocity.c

@@ -0,0 +1,149 @@
+#include "gps/NavEquations.h"
+
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+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;
+}

soc/software/gps_system/tests/test_gps_tracking_measurement.c

@@ -0,0 +1,179 @@
+#include "gps/gps_tracking_measurement.h"
+
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#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(&registers, 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(&registers, &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(&registers, &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;
+}