feat(gps): expose Doppler and signal quality measurements
This commit is contained in:
+247
-16
@@ -40,6 +40,14 @@ module tracking_wrapper #(
|
||||
import gps_top_pkg::*;
|
||||
import trck_pkg::*;
|
||||
|
||||
localparam int unsigned MEAS_CARRIER_WL =
|
||||
ACQUIRED_FREQ_IL + TRCK_CARR_NCO_FREQ_FL;
|
||||
localparam int unsigned MEAS_SUM_WL =
|
||||
TRCK_CORR_WL + $clog2(MS_PER_BIT);
|
||||
localparam int unsigned MEAS_WBP_WL =
|
||||
(2 * TRCK_CORR_WL) + 1 + $clog2(MS_PER_BIT);
|
||||
localparam int unsigned MEAS_COUNT_WL = $clog2(MS_PER_BIT + 1);
|
||||
|
||||
// apb state machine
|
||||
apb_state_e apb_bus_ps, apb_bus_ns;
|
||||
// tracking lock state machine
|
||||
@@ -70,6 +78,22 @@ module tracking_wrapper #(
|
||||
logic nav_bit, nav_bit_syned; // navigation bits.
|
||||
logic [31 : 0] abs_sample, abs_sample_syned;
|
||||
logic [REM_CODE_PH_I_WL-1 : 0] rem_code_ph, rem_code_ph_syned;
|
||||
logic measurement_valid, measurement_valid_syned;
|
||||
logic measurement_bit, measurement_bit_syned;
|
||||
logic [31:0] measurement_abs_sample, measurement_abs_sample_syned;
|
||||
logic [REM_CODE_PH_I_WL-1:0] measurement_rem_code_ph;
|
||||
logic [REM_CODE_PH_I_WL-1:0] measurement_rem_code_ph_syned;
|
||||
logic signed [MEAS_CARRIER_WL-1:0] measurement_carrier_freq_q7;
|
||||
logic [MEAS_CARRIER_WL-1:0] measurement_carrier_freq_q7_syned;
|
||||
logic signed [MEAS_SUM_WL-1:0] measurement_cn0_sum_i;
|
||||
logic signed [MEAS_SUM_WL-1:0] measurement_cn0_sum_q;
|
||||
logic [MEAS_SUM_WL-1:0] measurement_cn0_sum_i_syned;
|
||||
logic [MEAS_SUM_WL-1:0] measurement_cn0_sum_q_syned;
|
||||
logic [MEAS_WBP_WL-1:0] measurement_cn0_wbp;
|
||||
logic [MEAS_WBP_WL-1:0] measurement_cn0_wbp_syned;
|
||||
logic [MEAS_COUNT_WL-1:0] measurement_cn0_sample_count;
|
||||
logic [MEAS_COUNT_WL-1:0] measurement_cn0_sample_count_syned;
|
||||
logic [15:0] measurement_sequence, measurement_sequence_syned;
|
||||
logic preamble_ok;
|
||||
|
||||
// ( internal flags )
|
||||
@@ -77,6 +101,7 @@ module tracking_wrapper #(
|
||||
logic trck_nav_bit_rcv_flag_p, trck_nav_bit_rcv_flag_n;
|
||||
logic trck_timeout_flag_p, trck_timeout_flag_n;
|
||||
logic trck_nav_solve_flag_p, trck_nav_solve_flag_n;
|
||||
logic trck_measurement_flag_p, trck_measurement_flag_n;
|
||||
|
||||
logic [31 : 0] timeout_counter_p, timeout_counter_n; // tracking auto timeout value
|
||||
|
||||
@@ -94,6 +119,24 @@ module tracking_wrapper #(
|
||||
logic [31 : 0] nav_solve_timer_reg_p, nav_solve_timer_reg_n;
|
||||
logic [30 : 0] nav_bit_read_reg_p[10], nav_bit_read_reg_n[10];
|
||||
|
||||
// Atomic APB-domain copy of the 20 ms measurement.
|
||||
logic measurement_valid_r;
|
||||
logic measurement_capture_pending;
|
||||
logic [1:0] measurement_settle_count;
|
||||
logic measurement_capture;
|
||||
logic measurement_interrupt_pulse;
|
||||
logic measurement_data_valid_reg;
|
||||
logic measurement_bit_reg;
|
||||
logic measurement_lock_reg;
|
||||
logic [31:0] measurement_abs_sample_reg;
|
||||
logic [REM_CODE_PH_I_WL-1:0] measurement_rem_code_ph_reg;
|
||||
logic signed [MEAS_CARRIER_WL-1:0] measurement_carrier_freq_q7_reg;
|
||||
logic signed [MEAS_SUM_WL-1:0] measurement_cn0_sum_i_reg;
|
||||
logic signed [MEAS_SUM_WL-1:0] measurement_cn0_sum_q_reg;
|
||||
logic [MEAS_WBP_WL-1:0] measurement_cn0_wbp_reg;
|
||||
logic [MEAS_COUNT_WL-1:0] measurement_cn0_sample_count_reg;
|
||||
logic [15:0] measurement_sequence_reg;
|
||||
|
||||
|
||||
logic[2 : 0] start_trck_ctl_p, start_trck_ctl_n;
|
||||
logic[2 : 0] stop_trck_ctl_p, stop_trck_ctl_n;
|
||||
@@ -152,11 +195,55 @@ module tracking_wrapper #(
|
||||
.aresetn(PRESETN),
|
||||
.Din(rem_code_ph),
|
||||
.Dout(rem_code_ph_syned)
|
||||
);
|
||||
);
|
||||
|
||||
double_ff_syn#(1, 2) i_measurement_valid_syn(
|
||||
.clk(PCLK), .aresetn(PRESETN),
|
||||
.Din(measurement_valid), .Dout(measurement_valid_syned)
|
||||
);
|
||||
double_ff_syn#(1, 2) i_measurement_bit_syn(
|
||||
.clk(PCLK), .aresetn(PRESETN),
|
||||
.Din(measurement_bit), .Dout(measurement_bit_syned)
|
||||
);
|
||||
double_ff_syn#(32, 2) i_measurement_abs_sample_syn(
|
||||
.clk(PCLK), .aresetn(PRESETN),
|
||||
.Din(measurement_abs_sample), .Dout(measurement_abs_sample_syned)
|
||||
);
|
||||
double_ff_syn#(REM_CODE_PH_I_WL, 2) i_measurement_rem_code_ph_syn(
|
||||
.clk(PCLK), .aresetn(PRESETN),
|
||||
.Din(measurement_rem_code_ph), .Dout(measurement_rem_code_ph_syned)
|
||||
);
|
||||
double_ff_syn#(MEAS_CARRIER_WL, 2) i_measurement_carrier_syn(
|
||||
.clk(PCLK), .aresetn(PRESETN),
|
||||
.Din(measurement_carrier_freq_q7),
|
||||
.Dout(measurement_carrier_freq_q7_syned)
|
||||
);
|
||||
double_ff_syn#(MEAS_SUM_WL, 2) i_measurement_sum_i_syn(
|
||||
.clk(PCLK), .aresetn(PRESETN),
|
||||
.Din(measurement_cn0_sum_i), .Dout(measurement_cn0_sum_i_syned)
|
||||
);
|
||||
double_ff_syn#(MEAS_SUM_WL, 2) i_measurement_sum_q_syn(
|
||||
.clk(PCLK), .aresetn(PRESETN),
|
||||
.Din(measurement_cn0_sum_q), .Dout(measurement_cn0_sum_q_syned)
|
||||
);
|
||||
double_ff_syn#(MEAS_WBP_WL, 2) i_measurement_wbp_syn(
|
||||
.clk(PCLK), .aresetn(PRESETN),
|
||||
.Din(measurement_cn0_wbp), .Dout(measurement_cn0_wbp_syned)
|
||||
);
|
||||
double_ff_syn#(MEAS_COUNT_WL, 2) i_measurement_count_syn(
|
||||
.clk(PCLK), .aresetn(PRESETN),
|
||||
.Din(measurement_cn0_sample_count),
|
||||
.Dout(measurement_cn0_sample_count_syned)
|
||||
);
|
||||
double_ff_syn#(16, 2) i_measurement_sequence_syn(
|
||||
.clk(PCLK), .aresetn(PRESETN),
|
||||
.Din(measurement_sequence), .Dout(measurement_sequence_syned)
|
||||
);
|
||||
|
||||
trck_top i_trck_top(
|
||||
// common signals
|
||||
.trk_rstn(PRESETN),
|
||||
.afe_rstn(PRESETN),
|
||||
.trk_rstn(trck_resetn),
|
||||
.afe_rstn(afe_resetn),
|
||||
.trk_clk(trck_clk),
|
||||
.afe_clk(afe_clk),
|
||||
.sample_align_i(sample_align), // from afe
|
||||
@@ -173,15 +260,83 @@ module tracking_wrapper #(
|
||||
// output to regs. (all values are valid on bit_vld except lock ?)
|
||||
.bit_o(nav_bit),
|
||||
.bit_vld_o(bit_valid),
|
||||
.abs_sample_o(abs_sample), // 20 ms bit valid aligned
|
||||
.rem_code_ph_o(rem_code_ph),
|
||||
.lock_o(lock),
|
||||
.abs_sample_o(abs_sample), // 20 ms bit valid aligned
|
||||
.rem_code_ph_o(rem_code_ph),
|
||||
.measurement_vld_o(measurement_valid),
|
||||
.measurement_bit_o(measurement_bit),
|
||||
.measurement_abs_sample_o(measurement_abs_sample),
|
||||
.measurement_rem_code_ph_o(measurement_rem_code_ph),
|
||||
.measurement_carrier_freq_q7_o(measurement_carrier_freq_q7),
|
||||
.measurement_cn0_sum_i_o(measurement_cn0_sum_i),
|
||||
.measurement_cn0_sum_q_o(measurement_cn0_sum_q),
|
||||
.measurement_cn0_wbp_o(measurement_cn0_wbp),
|
||||
.measurement_cn0_sample_count_o(measurement_cn0_sample_count),
|
||||
.measurement_sequence_o(measurement_sequence),
|
||||
.lock_o(lock),
|
||||
.busy_o(busy),
|
||||
|
||||
//unconnected ( for debug purposes )
|
||||
.fifo_of_o()
|
||||
|
||||
);
|
||||
);
|
||||
|
||||
/*
|
||||
* The source payload is stable for 20 ms. Its valid marker and every data
|
||||
* bit pass through the same three-stage synchronizer; two extra PCLK cycles
|
||||
* after the synchronized rising edge provide a conservative bus-settle
|
||||
* margin before the software-visible registers and interrupt are updated.
|
||||
*/
|
||||
always_ff @(posedge PCLK or negedge PRESETN) begin : measurement_capture_seq
|
||||
if(!PRESETN) begin
|
||||
measurement_valid_r <= 1'b0;
|
||||
measurement_capture_pending <= 1'b0;
|
||||
measurement_settle_count <= '0;
|
||||
measurement_capture <= 1'b0;
|
||||
measurement_interrupt_pulse <= 1'b0;
|
||||
measurement_data_valid_reg <= 1'b0;
|
||||
measurement_bit_reg <= 1'b0;
|
||||
measurement_lock_reg <= 1'b0;
|
||||
measurement_abs_sample_reg <= '0;
|
||||
measurement_rem_code_ph_reg <= '0;
|
||||
measurement_carrier_freq_q7_reg <= '0;
|
||||
measurement_cn0_sum_i_reg <= '0;
|
||||
measurement_cn0_sum_q_reg <= '0;
|
||||
measurement_cn0_wbp_reg <= '0;
|
||||
measurement_cn0_sample_count_reg <= '0;
|
||||
measurement_sequence_reg <= '0;
|
||||
end else begin
|
||||
measurement_valid_r <= measurement_valid_syned;
|
||||
measurement_capture <= 1'b0;
|
||||
measurement_interrupt_pulse <= measurement_capture;
|
||||
|
||||
if(measurement_valid_syned && !measurement_valid_r) begin
|
||||
measurement_capture_pending <= 1'b1;
|
||||
measurement_settle_count <= 2;
|
||||
end else if(measurement_capture_pending) begin
|
||||
if(measurement_settle_count != 0) begin
|
||||
measurement_settle_count <= measurement_settle_count - 1'b1;
|
||||
end else begin
|
||||
measurement_capture_pending <= 1'b0;
|
||||
measurement_capture <= 1'b1;
|
||||
measurement_data_valid_reg <= 1'b1;
|
||||
measurement_bit_reg <= measurement_bit_syned;
|
||||
measurement_lock_reg <= lock_syned;
|
||||
measurement_abs_sample_reg <= measurement_abs_sample_syned;
|
||||
measurement_rem_code_ph_reg <= measurement_rem_code_ph_syned;
|
||||
measurement_carrier_freq_q7_reg <=
|
||||
$signed(measurement_carrier_freq_q7_syned);
|
||||
measurement_cn0_sum_i_reg <=
|
||||
$signed(measurement_cn0_sum_i_syned);
|
||||
measurement_cn0_sum_q_reg <=
|
||||
$signed(measurement_cn0_sum_q_syned);
|
||||
measurement_cn0_wbp_reg <= measurement_cn0_wbp_syned;
|
||||
measurement_cn0_sample_count_reg <=
|
||||
measurement_cn0_sample_count_syned;
|
||||
measurement_sequence_reg <= measurement_sequence_syned;
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
|
||||
@@ -214,11 +369,13 @@ module tracking_wrapper #(
|
||||
trck_timeout_flag_p <= '0;
|
||||
trck_nav_solve_flag_p <= '0;
|
||||
trck_nav_bit_rcv_flag_p <= '0;
|
||||
trck_measurement_flag_p <= '0;
|
||||
|
||||
// control reg bit fields set by reset.
|
||||
control_reg_p[TRCK_NAV_BITS_FLAG_ENABLE_POS] <= 1'b1;
|
||||
control_reg_p[TRCK_NAV_SOLVE_FLAG_EN_POS] <= 1'b1;
|
||||
control_reg_p[TRCK_STOP_ON_UNLOCK_FLAG_EN_POS] <= 1'b1;
|
||||
control_reg_p[TRCK_MEASUREMENT_FLAG_EN_POS] <= 1'b1;
|
||||
|
||||
foreach(nav_bit_read_reg_p[i])
|
||||
nav_bit_read_reg_p[i] <= '0;
|
||||
@@ -244,6 +401,7 @@ module tracking_wrapper #(
|
||||
trck_timeout_flag_p <= trck_timeout_flag_n;
|
||||
trck_nav_bit_rcv_flag_p <= trck_nav_bit_rcv_flag_n;
|
||||
trck_nav_solve_flag_p <= trck_nav_solve_flag_n;
|
||||
trck_measurement_flag_p <= trck_measurement_flag_n;
|
||||
foreach(nav_bit_read_reg_p[i])
|
||||
nav_bit_read_reg_p[i] <= nav_bit_read_reg_n[i];
|
||||
end
|
||||
@@ -409,6 +567,10 @@ module tracking_wrapper #(
|
||||
if((control_reg_p[TRCK_NAV_SOLVE_INTR_EN_POS]) & (nav_solve_timer_reached)) begin
|
||||
gps_frame_ready = '1;
|
||||
end
|
||||
// intr due to a coherent 20 ms Doppler/CN0 measurement.
|
||||
if(control_reg_p[TRCK_MEASUREMENT_INTR_EN_POS] && measurement_interrupt_pulse) begin
|
||||
gps_frame_ready = '1;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
@@ -419,6 +581,7 @@ module tracking_wrapper #(
|
||||
trck_timeout_flag_n = trck_timeout_flag_p; // (all internal clear signals are tied to generic int_clear signal)
|
||||
trck_nav_bit_rcv_flag_n = trck_nav_bit_rcv_flag_p;
|
||||
trck_nav_solve_flag_n = trck_nav_solve_flag_p;
|
||||
trck_measurement_flag_n = trck_measurement_flag_p;
|
||||
foreach(nav_bit_read_reg_n[i])
|
||||
nav_bit_read_reg_n[i] = nav_bit_read_reg_p[i];
|
||||
|
||||
@@ -443,12 +606,21 @@ module tracking_wrapper #(
|
||||
if(control_reg_p[TRCK_INTRPT_ENABLE_POS] && control_reg_p[TRCK_NAV_SOLVE_INTR_EN_POS]) intr_pending_n = '1;
|
||||
end
|
||||
|
||||
if(measurement_capture) begin
|
||||
if(control_reg_p[TRCK_MEASUREMENT_FLAG_EN_POS])
|
||||
trck_measurement_flag_n = '1;
|
||||
if(control_reg_p[TRCK_INTRPT_ENABLE_POS] &&
|
||||
control_reg_p[TRCK_MEASUREMENT_INTR_EN_POS])
|
||||
intr_pending_n = '1;
|
||||
end
|
||||
|
||||
// generic clear all interrupt pending flag and other flags.
|
||||
if(intr_clear) begin // all internal flag clear
|
||||
intr_pending_n = '0;
|
||||
trck_timeout_flag_n = '0;
|
||||
trck_nav_bit_rcv_flag_n = '0;
|
||||
trck_nav_solve_flag_n = '0;
|
||||
trck_measurement_flag_n = '0;
|
||||
end
|
||||
end : nav_bit_load_and_intr_flag_comb
|
||||
|
||||
@@ -515,6 +687,10 @@ module tracking_wrapper #(
|
||||
|
||||
if(PWDATA[TRCK_NAV_SOLVE_FLAG_EN_POS]) control_reg_n[TRCK_NAV_SOLVE_FLAG_EN_POS] = 1'b1;
|
||||
if(!PWDATA[TRCK_NAV_SOLVE_FLAG_EN_POS]) control_reg_n[TRCK_NAV_SOLVE_FLAG_EN_POS] = 1'b0;
|
||||
if(PWDATA[TRCK_MEASUREMENT_INTR_EN_POS]) control_reg_n[TRCK_MEASUREMENT_INTR_EN_POS] = 1'b1;
|
||||
if(!PWDATA[TRCK_MEASUREMENT_INTR_EN_POS]) control_reg_n[TRCK_MEASUREMENT_INTR_EN_POS] = 1'b0;
|
||||
if(PWDATA[TRCK_MEASUREMENT_FLAG_EN_POS]) control_reg_n[TRCK_MEASUREMENT_FLAG_EN_POS] = 1'b1;
|
||||
if(!PWDATA[TRCK_MEASUREMENT_FLAG_EN_POS]) control_reg_n[TRCK_MEASUREMENT_FLAG_EN_POS] = 1'b0;
|
||||
end
|
||||
(LOW_ADDRESS + PRN_NUMBER_REG_OFFSET):
|
||||
begin
|
||||
@@ -560,20 +736,23 @@ module tracking_wrapper #(
|
||||
(control_reg_p[TRCK_NAV_SOLVE_INTR_EN_POS] << trck_pkg::TRCK_NAV_SOLVE_INTR_EN_POS) |
|
||||
(control_reg_p[TRCK_NAV_BITS_FLAG_ENABLE_POS] << trck_pkg::TRCK_NAV_BITS_FLAG_ENABLE_POS) |
|
||||
(control_reg_p[TRCK_STOP_ON_UNLOCK_FLAG_EN_POS] << trck_pkg::TRCK_STOP_ON_UNLOCK_FLAG_EN_POS) |
|
||||
(control_reg_p[TRCK_NAV_SOLVE_FLAG_EN_POS] << trck_pkg::TRCK_NAV_SOLVE_FLAG_EN_POS)
|
||||
(control_reg_p[TRCK_NAV_SOLVE_FLAG_EN_POS] << trck_pkg::TRCK_NAV_SOLVE_FLAG_EN_POS) |
|
||||
(control_reg_p[TRCK_MEASUREMENT_INTR_EN_POS] << trck_pkg::TRCK_MEASUREMENT_INTR_EN_POS) |
|
||||
(control_reg_p[TRCK_MEASUREMENT_FLAG_EN_POS] << trck_pkg::TRCK_MEASUREMENT_FLAG_EN_POS)
|
||||
);
|
||||
end
|
||||
(LOW_ADDRESS + STATUS_REG_OFFSET):
|
||||
begin
|
||||
PRDATA = 32'(
|
||||
(busy_syned << TRCK_BUSY_POS) |
|
||||
(lock_syned << TRCK_LOCKED_POS) |
|
||||
(0 << TRCK_FIFO_OV_POS) | // fifo overflow is not considered for now.
|
||||
(intr_pending_p << TRCK_INTR_PEND_POS) |
|
||||
(trck_timeout_flag_p << TRCK_UNLOCK_FLAG_POS) |
|
||||
(trck_nav_bit_rcv_flag_p << TRCK_NAV_BITS_RCV_FLAG_POS) |
|
||||
(trck_nav_solve_flag_p << TRCK_NAV_SOLVE_FLAG_POS)
|
||||
);
|
||||
(busy_syned << TRCK_BUSY_POS) |
|
||||
(lock_syned << TRCK_LOCKED_POS) |
|
||||
(0 << TRCK_FIFO_OV_POS) | // fifo overflow is not considered for now.
|
||||
(intr_pending_p << TRCK_INTR_PEND_POS) |
|
||||
(trck_timeout_flag_p << TRCK_UNLOCK_FLAG_POS) |
|
||||
(trck_nav_bit_rcv_flag_p << TRCK_NAV_BITS_RCV_FLAG_POS) |
|
||||
(trck_nav_solve_flag_p << TRCK_NAV_SOLVE_FLAG_POS) |
|
||||
(trck_measurement_flag_p << TRCK_MEASUREMENT_FLAG_POS)
|
||||
);
|
||||
|
||||
end
|
||||
(LOW_ADDRESS + PRN_NUMBER_REG_OFFSET):
|
||||
@@ -653,6 +832,58 @@ module tracking_wrapper #(
|
||||
//PRDATA = rem_code_ph_reg_p;
|
||||
PRDATA = {{(32-REM_CODE_PH_I_WL){1'b0}}, rem_code_ph_reg_p};
|
||||
end
|
||||
(LOW_ADDRESS + MEASUREMENT_META_REG_OFFSET):
|
||||
begin
|
||||
PRDATA = {
|
||||
8'h01,
|
||||
11'b0,
|
||||
measurement_cn0_sample_count_reg,
|
||||
5'b0,
|
||||
measurement_lock_reg,
|
||||
measurement_bit_reg,
|
||||
measurement_data_valid_reg
|
||||
};
|
||||
end
|
||||
(LOW_ADDRESS + MEASUREMENT_SEQUENCE_REG_OFFSET):
|
||||
begin
|
||||
PRDATA = {16'b0, measurement_sequence_reg};
|
||||
end
|
||||
(LOW_ADDRESS + MEASUREMENT_CARRIER_REG_OFFSET):
|
||||
begin
|
||||
PRDATA = {{(32-MEAS_CARRIER_WL){
|
||||
measurement_carrier_freq_q7_reg[MEAS_CARRIER_WL-1]}},
|
||||
measurement_carrier_freq_q7_reg};
|
||||
end
|
||||
(LOW_ADDRESS + MEASUREMENT_ABS_SAMPLE_REG_OFFSET):
|
||||
begin
|
||||
PRDATA = measurement_abs_sample_reg;
|
||||
end
|
||||
(LOW_ADDRESS + MEASUREMENT_REM_CODE_REG_OFFSET):
|
||||
begin
|
||||
PRDATA = {{(32-REM_CODE_PH_I_WL){1'b0}},
|
||||
measurement_rem_code_ph_reg};
|
||||
end
|
||||
(LOW_ADDRESS + MEASUREMENT_SUM_I_REG_OFFSET):
|
||||
begin
|
||||
PRDATA = {{(32-MEAS_SUM_WL){
|
||||
measurement_cn0_sum_i_reg[MEAS_SUM_WL-1]}},
|
||||
measurement_cn0_sum_i_reg};
|
||||
end
|
||||
(LOW_ADDRESS + MEASUREMENT_SUM_Q_REG_OFFSET):
|
||||
begin
|
||||
PRDATA = {{(32-MEAS_SUM_WL){
|
||||
measurement_cn0_sum_q_reg[MEAS_SUM_WL-1]}},
|
||||
measurement_cn0_sum_q_reg};
|
||||
end
|
||||
(LOW_ADDRESS + MEASUREMENT_WBP_LO_REG_OFFSET):
|
||||
begin
|
||||
PRDATA = measurement_cn0_wbp_reg[31:0];
|
||||
end
|
||||
(LOW_ADDRESS + MEASUREMENT_WBP_HI_REG_OFFSET):
|
||||
begin
|
||||
PRDATA = {{(64-MEAS_WBP_WL){1'b0}},
|
||||
measurement_cn0_wbp_reg[MEAS_WBP_WL-1:32]};
|
||||
end
|
||||
endcase
|
||||
end
|
||||
end
|
||||
|
||||
+15
-3
@@ -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;
|
||||
|
||||
+11
@@ -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)
|
||||
|
||||
Vendored
+1
@@ -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
|
||||
|
||||
@@ -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),
|
||||
|
||||
+26
@@ -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)
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
+52
-1
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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.
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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(
|
||||
|
||||
@@ -17,7 +17,7 @@
|
||||
Returns:
|
||||
rho - [out] computed range ( pseudo range)
|
||||
*/
|
||||
void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_ephemeris_t* eph, ionoutc_t* ionoutc, double* xyz, t_temp_pos* tempPos, double* clockCorr, mat_type *satellitePositions, mat_type * velocities){
|
||||
void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_ephemeris_t* eph, ionoutc_t* ionoutc, double* xyz, t_temp_pos* tempPos, double* clockCorr, double* clockDrift, mat_type *satellitePositions, mat_type * velocities){
|
||||
|
||||
double pos[3], vel[3], clk[2];
|
||||
double los[3];
|
||||
@@ -28,7 +28,7 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph
|
||||
//gpstime_t * gpstime = (gpstime_t*) malloc(sizeof(gpstime_t));
|
||||
//gpstime->sec = eph->toe.sec;
|
||||
//gpstime->week = eph->toe.week;
|
||||
|
||||
|
||||
//CPP'ye GOREydi, KAPATILDI// double abs_sample[SVN] = { 66564551., 66525117.,66589459.,66666672.,66633917.,66700000. };
|
||||
//CPP'ye GOREydi, KAPATILDI// int codePhase = 2772; // TODO should be given by tracker. Şu anda matlab'dan alindi..
|
||||
|
||||
@@ -38,6 +38,10 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
satellitePositions->matrix[sat_no][i] = pos[i];
|
||||
velocities->matrix[sat_no][i] = vel[i];
|
||||
}
|
||||
if(clockDrift != NULL) {
|
||||
*clockDrift = clk[1];
|
||||
}
|
||||
/*for (i = 0; i < 3; i++) tempPos->sv_po_at_pseudo_obs[i] = pos[i];
|
||||
//memset(los, '0', sizeof(double));
|
||||
@@ -168,20 +172,6 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph
|
||||
double ek;
|
||||
double ekdot;
|
||||
double cek, sek;
|
||||
double pk;
|
||||
double c2pk, s2pk;
|
||||
double uk;
|
||||
double ukdot;
|
||||
double cuk, suk;
|
||||
double ok;
|
||||
double sok, cok;
|
||||
double ik;
|
||||
double ikdot;
|
||||
double sik, cik;
|
||||
double rk;
|
||||
double rkdot;
|
||||
double xpk, ypk;
|
||||
double xpkdot, ypkdot;
|
||||
int cnt = 0;
|
||||
|
||||
|
||||
@@ -210,8 +200,12 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph
|
||||
|
||||
ekdot = mkdot/( 1 - eph->ecc*cek );
|
||||
double tak = atan2(sqrt(1.0 - eph->ecc * eph->ecc) * sek, cek - eph->ecc);
|
||||
double takdot = sek * ekdot * (1.0 + eph->ecc * cos(tak)) / (sin(tak) * (1.0 - eph->ecc* cek));
|
||||
relativistic = -4.442807633E-10 * eph->ecc * eph->sqrta * sek;
|
||||
double takdot =
|
||||
sqrt(1.0 - eph->ecc * eph->ecc) * ekdot /
|
||||
(1.0 - eph->ecc * cek);
|
||||
const double relativistic_coefficient = -4.442807633E-10;
|
||||
relativistic =
|
||||
relativistic_coefficient * eph->ecc * eph->sqrta * sek;
|
||||
|
||||
double phi = tak + eph->omg0;
|
||||
phi = remainder(phi, 2 * PI);
|
||||
@@ -219,18 +213,59 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph
|
||||
double u = phi + eph->cus * sin(2.0 * phi) + eph->cuc * cos(2.0 * phi);
|
||||
double r = eph->A * (1.0 - eph->ecc * cos(ek)) + eph->crc * cos(2 * phi) + eph->crs * sin(2 * phi);
|
||||
double i = eph->inc0 + eph->idot * tk + eph->cic * cos(2 * phi) + eph->cis * sin(2 * phi);
|
||||
double udot = takdot +
|
||||
2.0 * (eph->cus * cos(2.0 * phi) -
|
||||
eph->cuc * sin(2.0 * phi)) * takdot;
|
||||
double rdot = eph->A * eph->ecc * sin(ek) * ekdot +
|
||||
2.0 * (eph->crs * cos(2.0 * phi) -
|
||||
eph->crc * sin(2.0 * phi)) * takdot;
|
||||
double idot = eph->idot +
|
||||
2.0 * (eph->cis * cos(2.0 * phi) -
|
||||
eph->cic * sin(2.0 * phi)) * takdot;
|
||||
|
||||
double Omega = eph->omgdot + (eph->omgkdot - OMEGA_EARTH) * tk - OMEGA_EARTH * eph->toe.sec;
|
||||
Omega = remainder(Omega + 2 * PI, 2 * PI);
|
||||
double Omega_dot = eph->omgkdot - OMEGA_EARTH;
|
||||
|
||||
pos[0] = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
|
||||
pos[1] = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega);
|
||||
pos[2] = sin(u) * r * sin(i);
|
||||
double x_orbit = r * cos(u);
|
||||
double y_orbit = r * sin(u);
|
||||
double x_orbit_dot = rdot * cos(u) - y_orbit * udot;
|
||||
double y_orbit_dot = rdot * sin(u) + x_orbit * udot;
|
||||
|
||||
pos[0] = x_orbit * cos(Omega) - y_orbit * cos(i) * sin(Omega);
|
||||
pos[1] = x_orbit * sin(Omega) + y_orbit * cos(i) * cos(Omega);
|
||||
pos[2] = y_orbit * sin(i);
|
||||
vel[0] =
|
||||
x_orbit_dot * cos(Omega) -
|
||||
x_orbit * sin(Omega) * Omega_dot -
|
||||
y_orbit_dot * cos(i) * sin(Omega) +
|
||||
y_orbit * sin(i) * idot * sin(Omega) -
|
||||
y_orbit * cos(i) * cos(Omega) * Omega_dot;
|
||||
vel[1] =
|
||||
x_orbit_dot * sin(Omega) +
|
||||
x_orbit * cos(Omega) * Omega_dot +
|
||||
y_orbit_dot * cos(i) * cos(Omega) -
|
||||
y_orbit * sin(i) * idot * cos(Omega) -
|
||||
y_orbit * cos(i) * sin(Omega) * Omega_dot;
|
||||
vel[2] = y_orbit_dot * sin(i) + y_orbit * cos(i) * idot;
|
||||
|
||||
// Satellite clock correction
|
||||
// relativistic correction
|
||||
//*clockCorr = eph->af0 + tk * (eph->af1 + tk * eph->af2) + relativistic - eph->tgd; //7.23706041149e-05
|
||||
*clockCorr = (eph->af2 * clock_tk + eph->af1) * clock_tk + eph->af0 - eph->tgd + relativistic;
|
||||
double clock_correction =
|
||||
(eph->af2 * clock_tk + eph->af1) * clock_tk +
|
||||
eph->af0 - eph->tgd + relativistic;
|
||||
double clock_drift =
|
||||
(2.0 * eph->af2 * clock_tk) + eph->af1 +
|
||||
relativistic_coefficient * eph->ecc * eph->sqrta *
|
||||
cek * ekdot;
|
||||
if(clockCorr != NULL) {
|
||||
*clockCorr = clock_correction;
|
||||
}
|
||||
if(clk != NULL) {
|
||||
clk[0] = clock_correction;
|
||||
clk[1] = clock_drift;
|
||||
}
|
||||
/*if (clk[0] != SECONDS_IN_HALF_WEEK) {
|
||||
*clockCorr = clk[0];//eph->af0 + tk * (eph->af1 + tk * eph->af2) + relativistic - eph->tgd;
|
||||
}
|
||||
@@ -268,7 +303,9 @@ static bool least_squares_ura_sigma_m(unsigned char ura_index, double* sigma_m)
|
||||
|
||||
static double least_squares_measurement_weight(unsigned char ura_index,
|
||||
double elevation_deg,
|
||||
bool elevation_valid)
|
||||
bool elevation_valid,
|
||||
double cn0_dbhz,
|
||||
bool cn0_valid)
|
||||
{
|
||||
const double dtr = 0.01745329251994329576923690768489;
|
||||
const double min_elevation_deg = 10.0;
|
||||
@@ -296,6 +333,25 @@ static double least_squares_measurement_weight(unsigned char ura_index,
|
||||
double elevation_sigma_m = elevation_sigma_zenith_m / sin_el;
|
||||
variance_m2 += elevation_sigma_m * elevation_sigma_m;
|
||||
}
|
||||
if(cn0_valid && isfinite(cn0_dbhz))
|
||||
{
|
||||
/*
|
||||
* Receiver-specific code-tracking model. At 45 dB-Hz the additional
|
||||
* sigma is 3 m; every 20 dB change scales amplitude-domain error by
|
||||
* ten. Bounds prevent one channel dominating before calibration.
|
||||
*/
|
||||
double cn0_sigma_m =
|
||||
3.0 * pow(10.0, (45.0 - cn0_dbhz) / 20.0);
|
||||
if(cn0_sigma_m < 0.5)
|
||||
{
|
||||
cn0_sigma_m = 0.5;
|
||||
}
|
||||
if(cn0_sigma_m > 30.0)
|
||||
{
|
||||
cn0_sigma_m = 30.0;
|
||||
}
|
||||
variance_m2 += cn0_sigma_m * cn0_sigma_m;
|
||||
}
|
||||
return 1.0 / variance_m2;
|
||||
}
|
||||
|
||||
@@ -527,6 +583,7 @@ static bool least_squares_build_postfit(const mat_type* satpos,
|
||||
const mat_type* obs,
|
||||
int satellite_count,
|
||||
const unsigned char* ura_indices,
|
||||
const double* cn0_dbhz,
|
||||
const ionoutc_t* ionoutc,
|
||||
double gps_tow,
|
||||
const double state[4],
|
||||
@@ -583,7 +640,10 @@ static bool least_squares_build_postfit(const mat_type* satpos,
|
||||
az->matrix[0][i] = topocentric.azim;
|
||||
el->matrix[0][i] = topocentric.elv;
|
||||
|
||||
double weight = least_squares_measurement_weight(ura_indices[i], topocentric.elv, true);
|
||||
double weight = least_squares_measurement_weight(
|
||||
ura_indices[i], topocentric.elv, true,
|
||||
(cn0_dbhz != NULL) ? cn0_dbhz[i] : NAN,
|
||||
(cn0_dbhz != NULL) && isfinite(cn0_dbhz[i]));
|
||||
if(!isfinite(weight) || (weight <= 0.0))
|
||||
{
|
||||
return false;
|
||||
@@ -629,6 +689,7 @@ static double gps_wls_last_solution[4] = {0.0, 0.0, 0.0, 0.0};
|
||||
static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber,
|
||||
mat_type * az, mat_type * el,
|
||||
const unsigned char* ura_indices,
|
||||
const double* cn0_dbhz,
|
||||
const ionoutc_t* ionoutc, double gps_tow,
|
||||
const double seed[4], bool seed_valid,
|
||||
gps_nav_solution_t* solution_out)
|
||||
@@ -748,7 +809,10 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
|
||||
initialize_all(0.0, A);
|
||||
for(int i = 0 ; i < ActiveSatellitesNumber ; i++)
|
||||
{
|
||||
absoluteWeights[i] = least_squares_measurement_weight(ura_indices[i], 0.0, false);
|
||||
absoluteWeights[i] = least_squares_measurement_weight(
|
||||
ura_indices[i], 0.0, false,
|
||||
(cn0_dbhz != NULL) ? cn0_dbhz[i] : NAN,
|
||||
(cn0_dbhz != NULL) && isfinite(cn0_dbhz[i]));
|
||||
}
|
||||
|
||||
bool seeded_from_last_solution = seed_valid && (seed != NULL);
|
||||
@@ -843,7 +907,10 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
|
||||
//TODO: topocent e giren matrix lerin elemanları doğru yükleniyor mu diye bakılacak. !!!!!!!!!!!!!!!!!1
|
||||
az->matrix[0][i] = topcent->azim;
|
||||
el->matrix[0][i] = topcent->elv;
|
||||
absoluteWeights[i] = least_squares_measurement_weight(ura_indices[i], topcent->elv, true);
|
||||
absoluteWeights[i] = least_squares_measurement_weight(
|
||||
ura_indices[i], topcent->elv, true,
|
||||
(cn0_dbhz != NULL) ? cn0_dbhz[i] : NAN,
|
||||
(cn0_dbhz != NULL) && isfinite(cn0_dbhz[i]));
|
||||
iono = klobuchar_delay_meters(ionoutc, gps_tow,
|
||||
phi_lambda_h->phi, phi_lambda_h->lambda,
|
||||
topcent->azim, topcent->elv);
|
||||
@@ -989,7 +1056,8 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
|
||||
|
||||
double residuals_m[GPS_TRCK_IP_COUNT];
|
||||
if(!least_squares_build_postfit(satpos, obs, ActiveSatellitesNumber,
|
||||
ura_indices, ionoutc, gps_tow,
|
||||
ura_indices, cn0_dbhz,
|
||||
ionoutc, gps_tow,
|
||||
posVec->matrix[0], A, az, el,
|
||||
residuals_m, absoluteWeights))
|
||||
{
|
||||
@@ -1234,6 +1302,7 @@ least_squares_fail:
|
||||
bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber,
|
||||
mat_type * az, mat_type * el,
|
||||
const unsigned char* ura_indices,
|
||||
const double* cn0_dbhz,
|
||||
const ionoutc_t* ionoutc, double gps_tow,
|
||||
gps_nav_solution_t* solution_out)
|
||||
{
|
||||
@@ -1246,7 +1315,8 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
|
||||
|
||||
gps_nav_solution_t full_solution;
|
||||
if(!least_squares_candidate(satpos, obs, ActiveSatellitesNumber,
|
||||
az, el, ura_indices, ionoutc, gps_tow,
|
||||
az, el, ura_indices, cn0_dbhz,
|
||||
ionoutc, gps_tow,
|
||||
gps_wls_last_solution,
|
||||
gps_wls_last_solution_valid,
|
||||
&full_solution))
|
||||
@@ -1278,6 +1348,7 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
|
||||
mat_type* subset_az = create_matrix(1, subset_count, "fdeAz");
|
||||
mat_type* subset_el = create_matrix(1, subset_count, "fdeEl");
|
||||
unsigned char subset_ura[GPS_TRCK_IP_COUNT];
|
||||
double subset_cn0[GPS_TRCK_IP_COUNT];
|
||||
if((subset_satpos == NULL) || (subset_obs == NULL) ||
|
||||
(subset_az == NULL) || (subset_el == NULL))
|
||||
{
|
||||
@@ -1307,6 +1378,9 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
|
||||
}
|
||||
subset_obs->matrix[0][subset_row] = obs->matrix[0][original_row];
|
||||
subset_ura[subset_row] = ura_indices[original_row];
|
||||
subset_cn0[subset_row] =
|
||||
(cn0_dbhz != NULL) ?
|
||||
cn0_dbhz[original_row] : NAN;
|
||||
subset_row++;
|
||||
}
|
||||
|
||||
@@ -1317,6 +1391,7 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
|
||||
gps_nav_solution_t candidate;
|
||||
bool candidate_ok = least_squares_candidate(subset_satpos, subset_obs, subset_count,
|
||||
subset_az, subset_el, subset_ura,
|
||||
subset_cn0,
|
||||
ionoutc, gps_tow,
|
||||
full_seed, true, &candidate);
|
||||
free_matrix(subset_satpos);
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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]);
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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(®isters, 0, sizeof(registers));
|
||||
|
||||
const int32_t sum_i = 20000;
|
||||
const int32_t sum_q = 1000;
|
||||
const uint64_t wbp =
|
||||
cn0_wbp_for_target(sum_i, sum_q, 20u, 45.0);
|
||||
const int32_t carrier_q7 =
|
||||
(int32_t)llround((GPS_TRCK_IF_CENTER_HZ + 2500.0) *
|
||||
GPS_TRCK_CARRIER_Q7_SCALE);
|
||||
|
||||
registers.PRN_NUMBER = 7u;
|
||||
registers.MEASUREMENT_META =
|
||||
(GPS_TRCK_MEAS_META_VERSION <<
|
||||
GPS_TRCK_MEAS_META_VERSION_SHIFT) |
|
||||
(20u << GPS_TRCK_MEAS_META_COUNT_SHIFT) |
|
||||
(1u << GPS_TRCK_MEAS_META_VALID_POS) |
|
||||
(1u << GPS_TRCK_MEAS_META_NAV_BIT_POS) |
|
||||
(1u << GPS_TRCK_MEAS_META_LOCK_POS);
|
||||
registers.MEASUREMENT_SEQUENCE = 0x1234u;
|
||||
registers.MEASUREMENT_CARRIER_Q7 = carrier_q7;
|
||||
registers.MEASUREMENT_ABS_SAMPLE = 0x89abcdefu;
|
||||
registers.MEASUREMENT_REM_CODE_PHASE = 555u;
|
||||
registers.MEASUREMENT_SUM_I = sum_i;
|
||||
registers.MEASUREMENT_SUM_Q = sum_q;
|
||||
registers.MEASUREMENT_WBP_LO = (uint32_t)wbp;
|
||||
registers.MEASUREMENT_WBP_HI = (uint32_t)(wbp >> 32);
|
||||
|
||||
gps_tracking_measurement_t measurement;
|
||||
assert(gps_tracking_measurement_read(®isters, &measurement));
|
||||
assert(measurement.valid);
|
||||
assert(measurement.locked);
|
||||
assert(measurement.nav_bit);
|
||||
assert(measurement.prn == 7u);
|
||||
assert(measurement.sequence == 0x1234u);
|
||||
assert(measurement.absolute_sample == 0x89abcdefu);
|
||||
assert(measurement.remaining_code_phase == 555u);
|
||||
assert(measurement.cn0_sample_count == 20u);
|
||||
assert(measurement.cn0_wbp == wbp);
|
||||
assert(close_to(measurement.cn0_dbhz, 45.0, 0.01));
|
||||
assert(close_to(measurement.doppler_hz, 2500.0, 1.0 / 128.0));
|
||||
|
||||
const double expected_range_rate =
|
||||
-2500.0 * ((double)SPEED_OF_LIGHT / GPS_L1_CARRIER_HZ);
|
||||
assert(close_to(
|
||||
measurement.pseudorange_rate_mps,
|
||||
expected_range_rate,
|
||||
1.0e-9));
|
||||
|
||||
registers.MEASUREMENT_META &=
|
||||
~(1u << GPS_TRCK_MEAS_META_LOCK_POS);
|
||||
assert(!gps_tracking_measurement_read(®isters, &measurement));
|
||||
assert(!measurement.valid);
|
||||
}
|
||||
|
||||
static void test_velocity_wls(void)
|
||||
{
|
||||
const double receiver_position[3] = {6378137.0, 0.0, 0.0};
|
||||
const double directions[TEST_SATELLITES][3] = {
|
||||
{ 1.0, 0.0, 0.0},
|
||||
{ 0.0, 1.0, 0.0},
|
||||
{ 0.0, 0.0, 1.0},
|
||||
{-0.5773502691896258, 0.5773502691896258, 0.5773502691896258},
|
||||
{ 0.7071067811865475, -0.7071067811865475, 0.0},
|
||||
{ 0.4082482904638631, 0.4082482904638631, -0.8164965809277261}
|
||||
};
|
||||
const double satellite_velocities[TEST_SATELLITES][3] = {
|
||||
{ 0.0, 2700.0, 100.0},
|
||||
{-900.0, 0.0, 2500.0},
|
||||
{1200.0, -2100.0, 0.0},
|
||||
{1800.0, 400.0, -800.0},
|
||||
{-500.0, 1900.0, 600.0},
|
||||
{ 700.0, -1200.0, 2200.0}
|
||||
};
|
||||
const double satellite_clock_drift[TEST_SATELLITES] = {
|
||||
1.0e-11, -2.0e-11, 0.5e-11, 1.5e-11, -0.7e-11, 2.2e-11
|
||||
};
|
||||
const double expected_velocity[3] = {12.0, -4.0, 1.5};
|
||||
const double expected_clock_drift_mps = 18.0;
|
||||
|
||||
double satellite_positions[TEST_SATELLITES][3];
|
||||
gps_tracking_measurement_t measurements[TEST_SATELLITES];
|
||||
memset(measurements, 0, sizeof(measurements));
|
||||
for(unsigned int satellite = 0u;
|
||||
satellite < TEST_SATELLITES;
|
||||
satellite++)
|
||||
{
|
||||
double los_satellite_velocity = 0.0;
|
||||
double los_receiver_velocity = 0.0;
|
||||
for(unsigned int component = 0u; component < 3u; component++)
|
||||
{
|
||||
satellite_positions[satellite][component] =
|
||||
receiver_position[component] +
|
||||
directions[satellite][component] * 21000000.0;
|
||||
los_satellite_velocity +=
|
||||
directions[satellite][component] *
|
||||
satellite_velocities[satellite][component];
|
||||
los_receiver_velocity +=
|
||||
directions[satellite][component] *
|
||||
expected_velocity[component];
|
||||
}
|
||||
measurements[satellite].valid = true;
|
||||
measurements[satellite].locked = true;
|
||||
measurements[satellite].cn0_dbhz = 42.0 + (double)satellite;
|
||||
measurements[satellite].pseudorange_rate_mps =
|
||||
los_satellite_velocity -
|
||||
los_receiver_velocity +
|
||||
expected_clock_drift_mps -
|
||||
(double)SPEED_OF_LIGHT *
|
||||
satellite_clock_drift[satellite];
|
||||
}
|
||||
|
||||
double solved_velocity[3];
|
||||
double solved_clock_drift;
|
||||
double residual_rms;
|
||||
assert(gps_tracking_velocity_wls(
|
||||
receiver_position,
|
||||
satellite_positions,
|
||||
satellite_velocities,
|
||||
satellite_clock_drift,
|
||||
measurements,
|
||||
TEST_SATELLITES,
|
||||
solved_velocity,
|
||||
&solved_clock_drift,
|
||||
&residual_rms));
|
||||
|
||||
for(unsigned int component = 0u; component < 3u; component++)
|
||||
{
|
||||
assert(close_to(
|
||||
solved_velocity[component],
|
||||
expected_velocity[component],
|
||||
1.0e-9));
|
||||
}
|
||||
assert(close_to(
|
||||
solved_clock_drift,
|
||||
expected_clock_drift_mps,
|
||||
1.0e-9));
|
||||
assert(residual_rms < 1.0e-9);
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
test_cn0_and_mailbox_decode();
|
||||
test_velocity_wls();
|
||||
puts("test_gps_tracking_measurement PASS");
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user