diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index c5bb55a76..3d159fd69 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -134,6 +134,9 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl // GPS L1 C/A does not have pilot component nor secondary code d_secondary = false; trk_parameters.track_pilot = false; + trk_parameters.slope = 1.0; + trk_parameters.spc = trk_parameters.early_late_space_chips; + trk_parameters.y_intercept = 1.0; // symbol integration: 20 trk symbols (20 ms) = 1 tlm bit // set the preamble in the secondary code acquisition to obtain tlm symbol synchronization d_secondary_code_length = static_cast(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); @@ -153,6 +156,9 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl // GPS L2 does not have pilot component nor secondary code d_secondary = false; trk_parameters.track_pilot = false; + trk_parameters.slope = 1.0; + trk_parameters.spc = trk_parameters.early_late_space_chips; + trk_parameters.y_intercept = 1.0; } else if (signal_type == "L5") { @@ -165,6 +171,9 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_samples_per_chip = 1; d_code_length_chips = static_cast(GPS_L5I_CODE_LENGTH_CHIPS); d_secondary = true; + trk_parameters.slope = 1.0; + trk_parameters.spc = trk_parameters.early_late_space_chips; + trk_parameters.y_intercept = 1.0; if (trk_parameters.track_pilot) { // synchronize pilot secondary code @@ -213,6 +222,9 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_correlation_length_ms = 4; d_code_samples_per_chip = 2; // CBOC disabled: 2 samples per chip. CBOC enabled: 12 samples per chip d_veml = true; + trk_parameters.spc = trk_parameters.early_late_space_chips; + trk_parameters.slope = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, trk_parameters.spc); + trk_parameters.y_intercept = GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, trk_parameters.spc); if (trk_parameters.track_pilot) { d_secondary = true; @@ -237,6 +249,9 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_samples_per_chip = 1; d_code_length_chips = static_cast(GALILEO_E5A_CODE_LENGTH_CHIPS); d_secondary = true; + trk_parameters.slope = 1.0; + trk_parameters.spc = trk_parameters.early_late_space_chips; + trk_parameters.y_intercept = 1.0; if (trk_parameters.track_pilot) { // synchronize pilot secondary code @@ -283,6 +298,9 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_samples_per_chip = 1; d_secondary = true; trk_parameters.track_pilot = false; + trk_parameters.slope = 1.0; + trk_parameters.spc = trk_parameters.early_late_space_chips; + trk_parameters.y_intercept = 1.0; // synchronize and remove data secondary code d_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); @@ -301,6 +319,9 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_samples_per_chip = 1; d_secondary = false; trk_parameters.track_pilot = false; + trk_parameters.slope = 1.0; + trk_parameters.spc = trk_parameters.early_late_space_chips; + trk_parameters.y_intercept = 1.0; d_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); @@ -965,7 +986,7 @@ void dll_pll_veml_tracking::run_dll_pll() } else { - d_code_error_chips = dll_nc_e_minus_l_normalized(d_E_accu, d_L_accu); // [chips/Ti] + d_code_error_chips = dll_nc_e_minus_l_normalized(d_E_accu, d_L_accu, trk_parameters.spc, trk_parameters.slope, trk_parameters.y_intercept); // [chips/Ti] } // Code discriminator filter d_code_error_filt_chips = d_code_loop_filter.apply(d_code_error_chips); // [chips/second] @@ -1621,6 +1642,12 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_E_accu = *d_Early; d_P_accu = *d_Prompt; d_L_accu = *d_Late; + trk_parameters.spc = trk_parameters.early_late_space_chips; + // if (std::string(trk_parameters.signal) == "E1") + // { + // trk_parameters.slope = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, trk_parameters.spc); + // trk_parameters.y_intercept = GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, trk_parameters.spc); + // } // fail-safe: check if the secondary code or bit synchronization has not succeeded in a limited time period if (trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) @@ -1722,11 +1749,18 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_local_code_shift_chips[1] = -trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); d_local_code_shift_chips[3] = trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); d_local_code_shift_chips[4] = trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + trk_parameters.spc = trk_parameters.early_late_space_narrow_chips; + // if (std::string(trk_parameters.signal) == "E1") + // { + // trk_parameters.slope = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, trk_parameters.spc); + // trk_parameters.y_intercept = GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, trk_parameters.spc); + // } } else { d_local_code_shift_chips[0] = -trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); d_local_code_shift_chips[2] = trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + trk_parameters.spc = trk_parameters.early_late_space_narrow_chips; } } else diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc index 2236667de..c7f341d6f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc @@ -129,6 +129,9 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & // GPS L1 C/A does not have pilot component nor secondary code d_secondary = false; trk_parameters.track_pilot = false; + trk_parameters.slope = 1.0; + trk_parameters.spc = trk_parameters.early_late_space_chips; + trk_parameters.y_intercept = 1.0; // symbol integration: 20 trk symbols (20 ms) = 1 tlm bit // set the preamble in the secondary code acquisition to obtain tlm symbol synchronization d_secondary_code_length = static_cast(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); @@ -143,6 +146,9 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & // GPS L2C has 1 trk symbol (20 ms) per tlm bit, no symbol integration required d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 20; + trk_parameters.slope = 1.0; + trk_parameters.spc = trk_parameters.early_late_space_chips; + trk_parameters.y_intercept = 1.0; // GPS L2 does not have pilot component nor secondary code d_secondary = false; trk_parameters.track_pilot = false; @@ -156,6 +162,9 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 1; d_secondary = true; + trk_parameters.slope = 1.0; + trk_parameters.spc = trk_parameters.early_late_space_chips; + trk_parameters.y_intercept = 1.0; if (d_extended_correlation_in_fpga == true) { if (trk_parameters.extend_correlation_symbols > 1) @@ -207,6 +216,9 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_symbols_per_bit = 1; d_correlation_length_ms = 4; d_veml = true; + trk_parameters.spc = trk_parameters.early_late_space_chips; + trk_parameters.slope = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, trk_parameters.spc); + trk_parameters.y_intercept = GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, trk_parameters.spc); if (trk_parameters.track_pilot) { d_secondary = true; @@ -229,6 +241,9 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_symbols_per_bit = 20; d_correlation_length_ms = 1; d_secondary = true; + trk_parameters.slope = 1.0; + trk_parameters.spc = trk_parameters.early_late_space_chips; + trk_parameters.y_intercept = 1.0; if (d_extended_correlation_in_fpga == true) { if (trk_parameters.extend_correlation_symbols > 1) @@ -718,7 +733,7 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() } else { - d_code_error_chips = dll_nc_e_minus_l_normalized(d_E_accu, d_L_accu); // [chips/Ti] + d_code_error_chips = dll_nc_e_minus_l_normalized(d_E_accu, d_L_accu, trk_parameters.spc, trk_parameters.slope, trk_parameters.y_intercept); // [chips/Ti] } // Code discriminator filter d_code_error_filt_chips = d_code_loop_filter.apply(d_code_error_chips); // [chips/second] @@ -1581,6 +1596,12 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_E_accu = *d_Early; d_P_accu = *d_Prompt; d_L_accu = *d_Late; + trk_parameters.spc = trk_parameters.early_late_space_chips; + // if (std::string(trk_parameters.signal) == "E1") + // { + // trk_parameters.slope = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, trk_parameters.spc); + // trk_parameters.y_intercept = GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, trk_parameters.spc); + // } // fail-safe: check if the secondary code or bit synchronization has not succeeded in a limited time period if (trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) @@ -1723,11 +1744,15 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_local_code_shift_chips[1] = -trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); d_local_code_shift_chips[3] = trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); d_local_code_shift_chips[4] = trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + trk_parameters.spc = trk_parameters.early_late_space_narrow_chips; + // trk_parameters.slope = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, trk_parameters.spc); + // trk_parameters.y_intercept = GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, trk_parameters.spc); } else { d_local_code_shift_chips[0] = -trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); d_local_code_shift_chips[2] = trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + trk_parameters.spc = trk_parameters.early_late_space_narrow_chips; } } else diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc index 9078e8112..e497ec550 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -697,7 +697,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at // ################## DLL ########################################################## // DLL discriminator - d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late + d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2], d_early_late_spc_chips, 1.0); // [chips/Ti] //early and late // Code discriminator filter d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); // input [chips/Ti] -> output [chips/second] d_code_error_filt_chips_Ti = d_code_error_filt_chips_s * CURRENT_INTEGRATION_TIME_S; diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc index 2390e0736..053c07c58 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc @@ -694,7 +694,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at // ################## DLL ########################################################## // DLL discriminator - d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(std::complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag()), std::complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag())); // [chips/Ti] //early and late + d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(std::complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag()), std::complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag()), d_early_late_spc_chips, 1.0); // [chips/Ti] //early and late // Code discriminator filter d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); // input [chips/Ti] -> output [chips/second] d_code_error_filt_chips_Ti = d_code_error_filt_chips_s * CURRENT_INTEGRATION_TIME_S; diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc index bfb89dcfa..50747fd7a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc @@ -555,7 +555,7 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut // ################## DLL ########################################################## // DLL discriminator - code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late + code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2], d_early_late_spc_chips, 1.0); // [chips/Ti] //early and late // Code discriminator filter code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc index 17dc560a6..f492f4b14 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc @@ -693,7 +693,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at // ################## DLL ########################################################## // DLL discriminator - d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late + d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2], d_early_late_spc_chips, 1.0); // [chips/Ti] //early and late // Code discriminator filter d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); // input [chips/Ti] -> output [chips/second] d_code_error_filt_chips_Ti = d_code_error_filt_chips_s * CURRENT_INTEGRATION_TIME_S; diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc index 821211c35..b798f2115 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc @@ -692,7 +692,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at // ################## DLL ########################################################## // DLL discriminator - d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(std::complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag()), std::complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag())); // [chips/Ti] //early and late + d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(std::complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag()), std::complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag()), d_early_late_spc_chips, 1.0); // [chips/Ti] //early and late // Code discriminator filter d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); // input [chips/Ti] -> output [chips/second] d_code_error_filt_chips_Ti = d_code_error_filt_chips_s * CURRENT_INTEGRATION_TIME_S; diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc index cfbd5d40a..d8d478959 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc @@ -556,7 +556,7 @@ int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut // ################## DLL ########################################################## // DLL discriminator - code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late + code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2], d_early_late_spc_chips, 1.0); // [chips/Ti] //early and late // Code discriminator filter code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc index 99fe843b1..6d1d318dd 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc @@ -373,7 +373,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut // ################## DLL ########################################################## // DLL discriminator - code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] // early and late + code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2], d_early_late_spc_chips, 1.0); // [chips/Ti] // early and late // Code discriminator filter code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips_Ti); // input [chips/Ti] -> output [chips/second] code_error_filt_secs_Ti = code_error_filt_chips * CURRENT_INTEGRATION_TIME_S / d_code_freq_chips; // [s/Ti] diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc index 7eb28d460..13461e8ce 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -737,7 +737,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus // New code Doppler frequency estimation based on carrier frequency estimation d_code_freq_chips = GPS_L1_CA_CODE_RATE_CPS + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_CPS) / GPS_L1_FREQ_HZ); // DLL discriminator - code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] early and late + code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2], d_early_late_spc_chips, 1.0); // [chips/Ti] early and late // Code discriminator filter code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index 210af0877..613380808 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -50,6 +50,9 @@ Dll_Pll_Conf::Dll_Pll_Conf() very_early_late_space_chips = 0.5; early_late_space_narrow_chips = 0.1; very_early_late_space_narrow_chips = 0.1; + slope = 1.0; + spc = 0.5; + y_intercept = 1.0; extend_correlation_symbols = 5; cn0_samples = FLAGS_cn0_samples; cn0_smoother_samples = 200; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.h b/src/algorithms/tracking/libs/dll_pll_conf.h index 53a553355..18438d55d 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.h +++ b/src/algorithms/tracking/libs/dll_pll_conf.h @@ -52,6 +52,9 @@ public: float very_early_late_space_chips; float early_late_space_narrow_chips; float very_early_late_space_narrow_chips; + float slope; + float spc; + float y_intercept; int32_t extend_correlation_symbols; bool high_dyn; int32_t cn0_samples; diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc index d5a58ef0c..48c3d975f 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc @@ -52,6 +52,9 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() very_early_late_space_chips = 0.5; early_late_space_narrow_chips = 0.1; very_early_late_space_narrow_chips = 0.1; + slope = 1.0; + spc = 0.5; + y_intercept = 1.0; extend_correlation_symbols = 5; cn0_samples = FLAGS_cn0_samples; cn0_smoother_samples = 200; diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h index 317bb7668..edb4a30c5 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h @@ -56,6 +56,9 @@ public: float very_early_late_space_chips; float early_late_space_narrow_chips; float very_early_late_space_narrow_chips; + float slope; + float spc; + float y_intercept; int32_t extend_correlation_symbols; bool high_dyn; int32_t cn0_samples; diff --git a/src/algorithms/tracking/libs/tracking_discriminators.cc b/src/algorithms/tracking/libs/tracking_discriminators.cc index c7a5e7fb7..448e7713f 100644 --- a/src/algorithms/tracking/libs/tracking_discriminators.cc +++ b/src/algorithms/tracking/libs/tracking_discriminators.cc @@ -23,7 +23,6 @@ #include "tracking_discriminators.h" #include "MATH_CONSTANTS.h" -#include // All the outputs are in RADIANS @@ -114,12 +113,12 @@ double pll_cloop_two_quadrant_atan(gr_complex prompt_s1) /* * DLL Noncoherent Early minus Late envelope normalized discriminator: * \f{equation} - * error=\frac{1}{2}\frac{E-L}{E+L}, + * error = \frac{y_{intercept} - \text{slope} * \epsilon}{\text{slope}} \frac{E-L}{E+L}, * \f} * where \f$E=\sqrt{I_{ES}^2+Q_{ES}^2}\f$ is the Early correlator output absolute value and * \f$L=\sqrt{I_{LS}^2+Q_{LS}^2}\f$ is the Late correlator output absolute value. The output is in [chips]. */ -double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1) +double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1, float spc, float slope, float y_intercept) { double P_early = std::abs(early_s1); double P_late = std::abs(late_s1); @@ -128,7 +127,7 @@ double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1) { return 0.0; } - return 0.5 * (P_early - P_late) / E_plus_L; + return ((y_intercept - slope * spc) / slope) * (P_early - P_late) / E_plus_L; } diff --git a/src/algorithms/tracking/libs/tracking_discriminators.h b/src/algorithms/tracking/libs/tracking_discriminators.h index 5ff2648f4..fcba833b8 100644 --- a/src/algorithms/tracking/libs/tracking_discriminators.h +++ b/src/algorithms/tracking/libs/tracking_discriminators.h @@ -28,6 +28,7 @@ #define GNSS_SDR_TRACKING_DISCRIMINATORS_H #include +#include /*! brief FLL four quadrant arctan discriminator * @@ -83,12 +84,12 @@ double pll_cloop_two_quadrant_atan(gr_complex prompt_s1); * * DLL Noncoherent Early minus Late envelope normalized discriminator: * \f{equation} - * error=\frac{E-L}{E+L}, + * error = \frac{y_{intercept} - \text{slope} * \epsilon}{\text{slope}} \frac{E-L}{E+L}, * \f} * where \f$E=\sqrt{I_{ES}^2+Q_{ES}^2}\f$ is the Early correlator output absolute value and * \f$L=\sqrt{I_{LS}^2+Q_{LS}^2}\f$ is the Late correlator output absolute value. The output is in [chips]. */ -double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1); +double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1, float spc = 0.5, float slope = 1.0, float y_intercept = 1.0); /*! \brief DLL Noncoherent Very Early Minus Late Power (VEMLP) normalized discriminator @@ -104,4 +105,91 @@ double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1); double dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1); +template +double CalculateSlope(Fun &&f, double x) +{ + static constexpr double dx = 1e-6; + + return (f(x + dx / 2.0) - f(x - dx / 2.0)) / dx; +} + +template +double CalculateSlopeAbs(Fun &&f, double x) +{ + static constexpr double dx = 1e-6; + + return (std::abs(f(x + dx / 2.0)) - std::abs(f(x - dx / 2.0))) / dx; +} + +template +double GetYIntercept(Fun &&f, double x) +{ + double slope = CalculateSlope(f, x); + double y1 = f(x); + + return y1 - slope * x; +} + +template +double GetYInterceptAbs(Fun &&f, double x) +{ + double slope = CalculateSlopeAbs(f, x); + double y1 = std::abs(f(x)); + return y1 - slope * x; +} + +// SinBocCorrelationFunction and CosBocCorrelationFunction from +// Sousa, F. and Nunes, F., "New Expressions for the Autocorrelation +// Function of BOC GNSS Signals", NAVIGATION - Journal of the Institute +// of Navigation, March 2013. +// +template +double SinBocCorrelationFunction(double offset_in_chips) +{ + static constexpr int TWO_P = 2 * M / N; + + double abs_tau = std::abs(offset_in_chips); + + if (abs_tau > 1.0) + { + return 0.0; + } + + int k = static_cast(std::ceil(TWO_P * abs_tau)); + + double sgn = ((k & 0x01) == 0 ? 1.0 : -1.0); // (-1)^k + + return sgn * (2.0 * (k * k - k * TWO_P - k) / TWO_P + 1.0 + + (2 * TWO_P - 2 * k + 1) * abs_tau); +} + + +template +double CosBocCorrelationFunction(double offset_in_chips) +{ + static constexpr int TWO_P = 2 * M / N; + + double abs_tau = std::abs(offset_in_chips); + + if (abs_tau > 1.0) + { + return 0.0; + } + + int k = static_cast(std::floor(2.0 * TWO_P * abs_tau)); + + if ((k & 0x01) == 0) // k is even + { + double sgn = ((k >> 1) & 0x01 ? -1.0 : 1.0); // (-1)^(k/2) + + return sgn * ((2 * k * TWO_P + 2 * TWO_P - k * k) / (2.0 * TWO_P) + (-2 * TWO_P + k - 1) * abs_tau); + } + else + { + double sgn = (((k + 1) >> 1) & 0x01 ? -1.0 : 1.0); // (-1)^((k+1)/2) + + return sgn * ((k * k + 2 * k - 2 * k * TWO_P + 1) / (2.0 * TWO_P) + (2 * TWO_P - k - 2) * abs_tau); + } +} + #endif diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index 22663d8d9..ec19fcbbf 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -94,12 +94,14 @@ DECLARE_string(log_dir); #endif #include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc" #include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc" +#include "unit-tests/signal-processing-blocks/tracking/discriminator_test.cc" #include "unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc" #include "unit-tests/signal-processing-blocks/tracking/galileo_e5a_tracking_test.cc" #include "unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_c_aid_tracking_test.cc" #include "unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_tracking_test.cc" #include "unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc" + #if CUDA_BLOCKS_TEST #include "unit-tests/signal-processing-blocks/tracking/gpu_multicorrelator_test.cc" #endif diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/discriminator_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/discriminator_test.cc new file mode 100644 index 000000000..efdc5a220 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/discriminator_test.cc @@ -0,0 +1,177 @@ +/*! + * \file discriminator_test.cc + * \brief This file implements tests for the tracking discriminators + * \author Cillian O'Driscoll, 2019. cillian.odriscoll(at)gmail.com + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ------------------------------------------------------------------------- + */ + +#include "tracking_discriminators.h" +#include +#include +#include + +double BpskCorrelationFunction(double offset_in_chips) +{ + double abs_tau = std::abs(offset_in_chips); + + if (abs_tau > 1.0) + { + return 0.0; + } + return 1.0 - abs_tau; +} + + +TEST(DllNcEMinusLNormalizedTest, Bpsk) +{ + std::vector complex_amplitude_vector = {{1.0, 0.0}, {-1.0, 0.0}, {0.0, 1.0}, {1.0, 1.0}}; + std::vector spacing_vector = {0.5, 0.25, 0.1, 0.01}; + std::vector error_vector = {0.0, 0.01, 0.1, 0.25, -0.25, -0.1, -0.01}; + + for (auto A : complex_amplitude_vector) + { + for (auto spacing : spacing_vector) + { + for (auto err : error_vector) + { + gr_complex E = A * static_cast(BpskCorrelationFunction(err - spacing)); + gr_complex L = A * static_cast(BpskCorrelationFunction(err + spacing)); + + double disc_out = dll_nc_e_minus_l_normalized(E, L, spacing); + + if (std::abs(err) < 2.0 * spacing) + { + EXPECT_NEAR(disc_out, err, 1e-4) << " Spacing: " << spacing; + } + else + { + EXPECT_TRUE(err * disc_out >= 0.0); + } + + if (spacing != 0.5 and err != 0.0) + { + double disc_out_old = dll_nc_e_minus_l_normalized(E, L); + + EXPECT_NE(disc_out_old, err); + } + } + } + } +} + + +TEST(DllNcEMinusLNormalizedTest, SinBoc11) +{ + std::vector complex_amplitude_vector = {{1.0, 0.0}, {-1.0, 0.0}, {0.0, 1.0}, {1.0, 1.0}}; + std::vector spacing_vector = {0.75, 0.6666, 5.0 / 12.0, 0.25, 1.0 / 6.0, 0.01}; + std::vector error_vector = {0.0, 0.01, 0.1, 0.25, -0.25, -0.1, -0.01}; + + for (auto A : complex_amplitude_vector) + { + for (auto spacing : spacing_vector) + { + double corr_slope = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, spacing); + double y_intercept = GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, spacing); + + for (auto err : error_vector) + { + gr_complex E = A * static_cast(SinBocCorrelationFunction<1, 1>(err - spacing)); + gr_complex L = A * static_cast(SinBocCorrelationFunction<1, 1>(err + spacing)); + + double disc_out = dll_nc_e_minus_l_normalized(E, L, spacing, corr_slope, y_intercept); + double corr_slope_at_err = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, spacing + err); + double corr_slope_at_neg_err = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, spacing - err); + + bool in_linear_region = (std::abs(err) < spacing) and (std::abs(corr_slope_at_err - corr_slope_at_neg_err) < 0.01); + double norm_factor = (y_intercept - corr_slope * spacing) / spacing; + + if (in_linear_region) + { + EXPECT_NEAR(disc_out, err, 1e-4) << " Spacing: " << spacing << ", slope : " << corr_slope << ", y_intercept: " << y_intercept << ", norm: " << norm_factor << " E: " << E << ", L: " << L; + if (norm_factor != 0.5 and err != 0.0) + { + double disc_out_old = dll_nc_e_minus_l_normalized(E, L); + EXPECT_NE(disc_out_old, err) << " Spacing: " << spacing << ", slope : " << corr_slope << ", y_intercept: " << y_intercept << ", norm: " << norm_factor << " E: " << E << ", L: " << L; + } + } + } + } + } +} + + +TEST(CosBocCorrelationFunction, FixedPoints) +{ + double res = CosBocCorrelationFunction<1, 1>(0.0); + EXPECT_NEAR(res, 1.0, 1e-4); + res = CosBocCorrelationFunction<1, 1>(0.2); + EXPECT_NEAR(res, 0.0, 1e-4); + res = CosBocCorrelationFunction<1, 1>(0.25); + EXPECT_NEAR(res, -0.25, 1e-4); + res = CosBocCorrelationFunction<1, 1>(0.5); + EXPECT_NEAR(res, -0.5, 1e-4); + res = CosBocCorrelationFunction<1, 1>(0.75); + EXPECT_NEAR(res, 0.25, 1e-4); + res = CosBocCorrelationFunction<1, 1>(1.0); + EXPECT_NEAR(res, 0.0, 1e-4); + res = CosBocCorrelationFunction<1, 1>(-0.2); + EXPECT_NEAR(res, 0.0, 1e-4); + res = CosBocCorrelationFunction<1, 1>(-0.5); + EXPECT_NEAR(res, -0.5, 1e-4); + res = CosBocCorrelationFunction<1, 1>(-0.75); + EXPECT_NEAR(res, 0.25, 1e-4); + res = CosBocCorrelationFunction<1, 1>(-1.0); + EXPECT_NEAR(res, 0.0, 1e-4); +} + + +TEST(DllNcEMinusLNormalizedTest, CosBoc11) +{ + std::vector complex_amplitude_vector = {{1.0, 0.0}, {-1.0, 0.0}, {0.0, 1.0}, {1.0, 1.0}}; + std::vector spacing_vector = {0.875, 0.588, 0.1, 0.01}; + std::vector error_vector = {0.0, 0.01, 0.1, 0.25, -0.25, -0.1, -0.01}; + + for (auto A : complex_amplitude_vector) + { + for (auto spacing : spacing_vector) + { + double corr_slope = -CalculateSlopeAbs(&CosBocCorrelationFunction<1, 1>, spacing); + double y_intercept = GetYInterceptAbs(&CosBocCorrelationFunction<1, 1>, spacing); + for (auto err : error_vector) + { + gr_complex E = A * static_cast(CosBocCorrelationFunction<1, 1>(err - spacing)); + gr_complex L = A * static_cast(CosBocCorrelationFunction<1, 1>(err + spacing)); + + double disc_out = dll_nc_e_minus_l_normalized(E, L, spacing, corr_slope, y_intercept); + double corr_slope_at_err = -CalculateSlopeAbs(&CosBocCorrelationFunction<1, 1>, spacing + err); + double corr_slope_at_neg_err = -CalculateSlopeAbs(&CosBocCorrelationFunction<1, 1>, spacing - err); + + bool in_linear_region = (std::abs(err) < spacing) and (std::abs(corr_slope_at_err - corr_slope_at_neg_err) < 0.01); + double norm_factor = (y_intercept - corr_slope * spacing) / spacing; + + if (in_linear_region) + { + EXPECT_NEAR(disc_out, err, 1e-4) << " Spacing: " << spacing << ", slope : " << corr_slope << ", y_intercept: " << y_intercept << ", norm: " << norm_factor << " E: " << E << ", L: " << L; + if (norm_factor != 0.5 and err != 0.0) + { + double disc_out_old = dll_nc_e_minus_l_normalized(E, L); + EXPECT_NE(disc_out_old, err) << " Spacing: " << spacing << ", slope : " << corr_slope << ", y_intercept: " << y_intercept << ", norm: " << norm_factor << " E: " << E << ", L: " << L; + } + } + } + } + } +}