From 33d111524660e5fd529c9041ee0f25d765131714 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Tue, 18 Jun 2019 18:22:01 +0200 Subject: [PATCH 01/22] added support for extended coherent integration in the FPGA. The code still needs to be optimized and cleaned. --- .../gnuradio_blocks/pcps_acquisition_fpga.cc | 69 + .../acquisition/libs/fpga_acquisition.cc | 1 + .../galileo_e1_dll_pll_veml_tracking_fpga.cc | 1 + .../galileo_e5a_dll_pll_tracking_fpga.cc | 8 + .../gps_l1_ca_dll_pll_tracking_fpga.cc | 2 +- .../adapters/gps_l5_dll_pll_tracking_fpga.cc | 1 + .../dll_pll_veml_tracking_fpga.cc | 1140 ++++++++++++++++- .../dll_pll_veml_tracking_fpga.h | 73 +- .../tracking/libs/dll_pll_conf_fpga.cc | 1 + .../tracking/libs/dll_pll_conf_fpga.h | 1 + .../tracking/libs/fpga_multicorrelator.cc | 271 +++- .../tracking/libs/fpga_multicorrelator.h | 27 + 12 files changed, 1562 insertions(+), 33 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc index 99de2d8ed..17fbe2ca4 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc @@ -134,6 +134,58 @@ void pcps_acquisition_fpga::set_state(int32_t state) void pcps_acquisition_fpga::send_positive_acquisition() { + + // debug L5 +// d_gnss_synchro->Acq_delay_samples = 2694; +// d_gnss_synchro->Acq_doppler_hz = 2650; +// d_gnss_synchro->Acq_samplestamp_samples = 56500224; +// d_gnss_synchro->Flag_valid_word = 0; +// d_gnss_synchro->Flag_valid_pseudorange = 0; +// d_gnss_synchro->Flag_valid_symbol_output = 0; +// d_gnss_synchro->Flag_valid_acquisition = 0; + +// d_gnss_synchro->Acq_delay_samples = 10846; +// d_gnss_synchro->Acq_doppler_hz = 2575; +// d_gnss_synchro->Acq_samplestamp_samples = 399605760; +// d_gnss_synchro->Flag_valid_word = 0; +// d_gnss_synchro->Flag_valid_pseudorange = 0; +// d_gnss_synchro->Flag_valid_symbol_output = 0; +// d_gnss_synchro->Flag_valid_acquisition = 0; + +// if (d_channel == 0) +// { +// d_gnss_synchro->Acq_delay_samples = 401; +// d_gnss_synchro->Acq_doppler_hz = 2650; +// d_gnss_synchro->Acq_samplestamp_samples = 96591872; +// d_gnss_synchro->Flag_valid_word = 0; +// d_gnss_synchro->Flag_valid_pseudorange = 0; +// d_gnss_synchro->Flag_valid_symbol_output = 0; +// d_gnss_synchro->Flag_valid_acquisition = 0; + +// d_gnss_synchro->Acq_delay_samples = 1505; +// d_gnss_synchro->Acq_doppler_hz = 2575; +// d_gnss_synchro->Acq_samplestamp_samples = 194265553; +// d_gnss_synchro->Flag_valid_word = 0; +// d_gnss_synchro->Flag_valid_pseudorange = 0; +// d_gnss_synchro->Flag_valid_symbol_output = 0; +// d_gnss_synchro->Flag_valid_acquisition = 0; + +// } + + + // debug E5a +// d_gnss_synchro->Acq_delay_samples = 2012; +// d_gnss_synchro->Acq_doppler_hz = -1125; +// d_gnss_synchro->Acq_samplestamp_samples = 363462656; +// d_gnss_synchro->Flag_valid_word = 0; +// d_gnss_synchro->Flag_valid_pseudorange = 0; +// d_gnss_synchro->Flag_valid_symbol_output = 0; +// d_gnss_synchro->Flag_valid_acquisition = 0; + + + + + // Declare positive acquisition using a message port // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL DLOG(INFO) << "positive acquisition" @@ -146,6 +198,23 @@ void pcps_acquisition_fpga::send_positive_acquisition() << ", magnitude " << d_mag << ", input signal power " << d_input_power; + + std::cout << "positive acquisition" + << ", satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN + << ", sample_stamp " << d_sample_counter + << ", test statistics value " << d_test_statistics + << ", test statistics threshold " << d_threshold + << ", code phase " << d_gnss_synchro->Acq_delay_samples + << ", doppler " << d_gnss_synchro->Acq_doppler_hz + << ", magnitude " << d_mag + << ", input signal power " << d_input_power + << ", d_gnss_synchro->Acq_samplestamp_samples " << d_gnss_synchro->Acq_samplestamp_samples + << ", d_gnss_synchro->Flag_valid_word " << d_gnss_synchro->Flag_valid_word + << ", Flag_valid_pseudorange " << d_gnss_synchro->Flag_valid_pseudorange + << ", d_gnss_synchro->Flag_valid_symbol_output " << d_gnss_synchro->Flag_valid_symbol_output + << ", d_gnss_synchro->Flag_valid_acquisition " << d_gnss_synchro->Flag_valid_acquisition + << std::endl; + //the channel FSM is set, so, notify it directly the positive acquisition to minimize delays d_channel_fsm.lock()->Event_valid_acquisition(); } diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.cc b/src/algorithms/acquisition/libs/fpga_acquisition.cc index 00b1b65b3..f1fef24c5 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.cc +++ b/src/algorithms/acquisition/libs/fpga_acquisition.cc @@ -303,6 +303,7 @@ void Fpga_Acquisition::close_device() void Fpga_Acquisition::reset_acquisition(void) { + //printf("============ resetting the hw now from the acquisition ==============="); d_map_base[8] = RESET_ACQUISITION; // writing a 2 to d_map_base[8] resets the acquisition. This causes a reset of all // the FPGA HW modules including the multicorrelators } diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc index ac916cef3..94619adb1 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc @@ -280,6 +280,7 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( trk_param_fpga.data_codes = d_data_codes; trk_param_fpga.code_length_chips = GALILEO_E1_B_CODE_LENGTH_CHIPS; trk_param_fpga.code_samples_per_chip = code_samples_per_chip; // 2 sample per chip + trk_param_fpga.extended_correlation_in_fpga = false; //################# MAKE TRACKING GNURadio object ################### tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index 5f3241696..b7749a45b 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -262,6 +262,14 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( trk_param_fpga.data_codes = d_data_codes; trk_param_fpga.code_length_chips = code_length_chips; trk_param_fpga.code_samples_per_chip = code_samples_per_chip; // 2 sample per chip + if (d_track_pilot) + { + trk_param_fpga.extended_correlation_in_fpga = true; + } + else + { + trk_param_fpga.extended_correlation_in_fpga = false; + } //################# MAKE TRACKING GNURadio object ################### tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index e359dba88..90cebf4a8 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -229,7 +229,7 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( trk_param_fpga.ca_codes = d_ca_codes; trk_param_fpga.code_length_chips = GPS_L1_CA_CODE_LENGTH_CHIPS; trk_param_fpga.code_samples_per_chip = 1; // 1 sample per chip - + trk_param_fpga.extended_correlation_in_fpga = false; //################# MAKE TRACKING GNURadio object ################### tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index 1dca0c03a..d94e96443 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -286,6 +286,7 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( trk_param_fpga.data_codes = d_data_codes; trk_param_fpga.code_length_chips = code_length_chips; trk_param_fpga.code_samples_per_chip = code_samples_per_chip; // 2 sample per chip + trk_param_fpga.extended_correlation_in_fpga = true; tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; DLOG(INFO) << "tracking(" << tracking_fpga_sc->unique_id() << ")"; 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 5ee4bee2a..8b253f9b5 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 @@ -67,6 +67,7 @@ namespace fs = std::filesystem; namespace fs = boost::filesystem; #endif +#define MAX_INTEGR_TIME_PRN_SYMB 20 dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_) { @@ -75,7 +76,7 @@ dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Co dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_) : gr::block("dll_pll_veml_tracking_fpga", gr::io_signature::make(0, 0, sizeof(lv_16sc_t)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) + gr::io_signature::make(1, MAX_INTEGR_TIME_PRN_SYMB, sizeof(Gnss_Synchro))) { trk_parameters = conf_; // Telemetry bit synchronization message port input @@ -95,6 +96,13 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_code_chip_rate = 0.0; d_secondary_code_length = 0U; d_secondary_code_string = nullptr; + + // used by the FPGA + d_secondary_code_length_data = 0U; + d_secondary_code_string_data = 0U; //nullptr; + d_secondary_code_length_pilot = 0U; + d_secondary_code_string_pilot = 0U; //nullptr; + d_preambles_symbols = nullptr; d_preamble_length_symbols = 0; signal_type = std::string(trk_parameters.signal); @@ -113,6 +121,15 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_code_samples_per_chip = trk_parameters.code_samples_per_chip; // number of samples per chip d_code_length_chips = trk_parameters.code_length_chips; + d_extended_correlation_in_fpga = trk_parameters.extended_correlation_in_fpga; + + printf("d_extended_correlation_in_fpga = %d\n", d_extended_correlation_in_fpga); + d_sc_remodulate_enabled = false; // by default + + d_sc_demodulate_enabled = false; + + d_flag_printout = false; + if (trk_parameters.system == 'G') { systemName = "GPS"; @@ -152,6 +169,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & } d_symbol_history.set_capacity(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size d_symbol_history.clear(); // Clear all the elements in the buffer + } else if (signal_type == "2S") { @@ -173,20 +191,58 @@ 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; + + if (d_extended_correlation_in_fpga == true) + { + if (trk_parameters.extend_correlation_symbols > 1) + { + //printf("EXTENDED CORRELATION IN FPGA ENABLED => ENABLING SECONDARY CODE REMODULATION\n"); + d_sc_demodulate_enabled = true; + d_sc_remodulate_enabled = true; + } + } + if (trk_parameters.track_pilot) { d_secondary_code_length = static_cast(GPS_L5Q_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5Q_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "Q"; interchange_iq = true; + + // used by the FPGA + d_secondary_code_length_data = static_cast(GPS_L5I_NH_CODE_LENGTH); + d_secondary_code_string_data = const_cast(&GPS_L5I_NH_CODE_STR); + + d_secondary_code_length_pilot = static_cast(GPS_L5Q_NH_CODE_LENGTH); + d_secondary_code_string_pilot = const_cast(&GPS_L5Q_NH_CODE_STR); + + d_secondary_code_string_post_apply = const_cast(&GPS_L5I_NH_CODE_STR); + d_secondary_code_post_apply_length = static_cast(GPS_L5I_NH_CODE_LENGTH); + } else { + d_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "I"; interchange_iq = false; + + // used by the FPGA + d_secondary_code_length_data = static_cast(GPS_L5I_NH_CODE_LENGTH); + d_secondary_code_string_data = const_cast(&GPS_L5I_NH_CODE_STR); + + // debug + //printf("warning : secondary code going to the FPGA is bit flipped for debugging\n"); + //d_secondary_code_string_data = const_cast(&GPS_L5I_NH_CODE_STR_TEST); + + + d_secondary_code_string_post_apply = const_cast(&GPS_L5I_NH_CODE_STR); + d_secondary_code_post_apply_length = static_cast(GPS_L5I_NH_CODE_LENGTH); + } + + } else { @@ -202,7 +258,9 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & } else if (trk_parameters.system == 'E') { - systemName = "Galileo"; + + + systemName = "Galileo"; if (signal_type == "1B") { d_signal_carrier_freq = GALILEO_E1_FREQ_HZ; @@ -227,26 +285,94 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & } else if (signal_type == "5X") { + + + d_signal_carrier_freq = GALILEO_E5A_FREQ_HZ; d_code_period = GALILEO_E5A_CODE_PERIOD; d_code_chip_rate = GALILEO_E5A_CODE_CHIP_RATE_HZ; d_symbols_per_bit = 20; d_correlation_length_ms = 1; +// if (d_extended_correlation_in_fpga == true) +// { +// if (trk_parameters.extend_correlation_symbols > 1) +// { +// //printf("EXTENDED CORRELATION IN FPGA ENABLED => ENABLING SECONDARY CODE REMODULATION\n"); +// d_sc_remodulate_enabled = true; +// } +// } + + if (trk_parameters.track_pilot) { + + if (d_extended_correlation_in_fpga == true) + { + if (trk_parameters.extend_correlation_symbols > 1) + { + //printf("EXTENDED CORRELATION IN FPGA ENABLED => ENABLING SECONDARY CODE REMODULATION\n"); + d_sc_demodulate_enabled = true; + d_sc_remodulate_enabled = true; + } + } + d_secondary = true; d_secondary_code_length = static_cast(GALILEO_E5A_Q_SECONDARY_CODE_LENGTH); + signal_pretty_name = signal_pretty_name + "Q"; interchange_iq = true; + + // used by the FPGA + d_secondary_code_length_data = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_secondary_code_string_data = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); + + d_secondary_code_length_pilot = static_cast(GALILEO_E5A_Q_SECONDARY_CODE_LENGTH); + + // the data of the pilot secondary code depends on PRN and they are initialized later + + d_secondary_code_string_post_apply = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); + d_secondary_code_post_apply_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + + } else { + + + //d_flag_printout = true; + //Do not acquire secondary code in data component. It is done in telemetry decoder d_secondary = false; signal_pretty_name = signal_pretty_name + "I"; interchange_iq = false; + + // the coherent integration in the fpga when using E5a without tracking the pilot cannot be done + // until the bug in the initialization variables related to the acquisition of the secondary code is + // solved in the SW + +// printf("################# SET EXTENDED CORRELATION TO ZERO\n"); +// d_extended_correlation_in_fpga == false; +// d_sc_demodulate_enabled = false; +// d_sc_remodulate_enabled = false; + +// if (d_extended_correlation_in_fpga == true) +// { +// d_extended_correlation_in_fpga == false; +// } + // used by the FPGA + //d_secondary_code_length_data = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + //d_secondary_code_string_data = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); + + //d_secondary_code_string_post_apply = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); + //d_secondary_code_post_apply_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + + } + + + + } else { @@ -326,11 +452,13 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & if (trk_parameters.extend_correlation_symbols > 1) { d_enable_extended_integration = true; + printf("d_enable_extended_integration enabled\n"); } else { d_enable_extended_integration = false; trk_parameters.extend_correlation_symbols = 1; + printf("d_enable_extended_integration disabled\n"); } // --- Initializations --- @@ -339,6 +467,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_code_freq_chips = d_code_chip_rate; // Residual code phase (in chips) d_rem_code_phase_samples = 0.0; + d_rem_code_phase_samples_prev = 0.0; // Residual carrier phase d_rem_carr_phase_rad = 0.0; @@ -347,8 +476,21 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_acq_sample_stamp = 0ULL; d_absolute_samples_offset = 0ULL; - d_current_prn_length_samples = static_cast(trk_parameters.vector_length); - d_next_prn_length_samples = d_current_prn_length_samples; + //d_current_prn_length_samples = static_cast(trk_parameters.vector_length); + //d_next_prn_length_samples = d_current_prn_length_samples; + + d_current_integration_length_samples = static_cast(trk_parameters.vector_length); + d_next_integration_length_samples = d_current_integration_length_samples; + + d_extended_integration_first_prn_length_samples = static_cast(trk_parameters.vector_length); + d_extended_integration_next_prn_length_samples = d_extended_integration_first_prn_length_samples; + + //d_extended_integration_first_rem_carr_phase_rad = 0.0; + //d_extended_integration_next_rem_carr_phase_rad_step = 0.0; + + d_extended_integration_first_acc_carrier_phase_rad = 0.0; + d_extended_integration_next_acc_carrier_phase_rad_step = 0.0; + d_current_correlation_time_s = 0.0; d_correlation_length_samples = static_cast(trk_parameters.vector_length); // this one is only for initialisation and does not change its value (MM) @@ -438,6 +580,14 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & multicorrelator_fpga = std::make_shared(d_n_correlator_taps, device_name, device_base, ca_codes, data_codes, d_code_length_chips, trk_parameters.track_pilot, multicorr_type, d_code_samples_per_chip); multicorrelator_fpga->set_output_vectors(d_correlator_outs, d_Prompt_Data); d_sample_counter_next = 0ULL; + + d_num_current_syncrho_repetitions = 1; + + + // debug - erase previous outfile contents + std::ofstream outfile; + outfile.open("trk_out.txt", std::ios_base::trunc); + outfile.close(); } void dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) @@ -583,6 +733,7 @@ bool dll_pll_veml_tracking_fpga::acquire_secondary() bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integration_time_s) { + // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### if (d_cn0_estimation_counter < trk_parameters.cn0_samples) { @@ -602,6 +753,7 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra // Loss of lock detection if (!d_pull_in_transitory) { + //printf("entering d_pull_in_transitory\n"); if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < trk_parameters.cn0_min) { d_carrier_lock_fail_counter++; @@ -616,6 +768,7 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra } if (d_carrier_lock_fail_counter > trk_parameters.max_lock_fail) { + printf("LOSS OF LOCK (TRACKING) --------------------------------------------x\n"); std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock @@ -647,7 +800,7 @@ void dll_pll_veml_tracking_fpga::do_correlation_step(void) static_cast(d_rem_code_phase_chips) * static_cast(d_code_samples_per_chip), static_cast(d_code_phase_step_chips) * static_cast(d_code_samples_per_chip), static_cast(d_code_phase_rate_step_chips) * static_cast(d_code_samples_per_chip), - d_current_prn_length_samples); + d_current_integration_length_samples); } @@ -738,16 +891,31 @@ void dll_pll_veml_tracking_fpga::clear_tracking_vars() void dll_pll_veml_tracking_fpga::update_tracking_vars() { - T_chip_seconds = 1.0 / d_code_freq_chips; - T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); + +// // debug -- remove when done ! +// d_extended_integration_first_prn_length_samples = d_current_integration_length_samples - (trk_parameters.extend_correlation_symbols - 1)*static_cast(std::floor(T_prn_samples)); +// d_extended_integration_next_prn_length_samples = static_cast(std::floor(T_prn_samples)); +// // end of debug -- remove when done! + + T_chip_seconds = 1.0 / d_code_freq_chips; // d_code_freq_chips updated in dll-pll + T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); // d_code_freq_chips fixed permanently // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### // keep alignment parameters for the next input buffer // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + //T_prn_samples_prev = T_prn_samples; + T_prn_samples_prev = T_prn_samples; T_prn_samples = T_prn_seconds * trk_parameters.fs_in; - K_blk_samples = T_prn_samples + d_rem_code_phase_samples; + K_blk_samples_prev = K_blk_samples; + K_blk_samples = T_prn_samples + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function //d_next_prn_length_samples = static_cast(std::floor(K_blk_samples)); // round to a discrete number of samples - d_next_prn_length_samples = static_cast(std::floor(K_blk_samples)); // round to a discrete number of samples + //d_next_prn_length_samples = static_cast(std::floor(K_blk_samples)); // round to a discrete number of samples + + d_actual_blk_length = static_cast(std::floor(K_blk_samples)); + d_next_integration_length_samples = 2*d_actual_blk_length - d_current_integration_length_samples; + //d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); + + //int32_t actual_prn_length_samples = static_cast(std::floor(K_blk_samples)); //d_next_prn_length_samples = actual_prn_length_samples + (actual_prn_length_samples - d_current_prn_length_samples); @@ -757,7 +925,7 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() // carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2] if (trk_parameters.high_dyn) { - d_carr_ph_history.push_back(std::pair(d_carrier_phase_step_rad, static_cast(d_current_prn_length_samples))); + d_carr_ph_history.push_back(std::pair(d_carrier_phase_step_rad, static_cast(d_current_integration_length_samples))); if (d_carr_ph_history.full()) { double tmp_cp1 = 0.0; @@ -776,21 +944,25 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() } //std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples)); + d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); + + // debug + debug_d_rem_carr_phase_rad = d_rem_carr_phase_rad; + d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); // carrier phase accumulator //double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); //double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); //std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl; - d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples)); + d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); //################### DLL COMMANDS ################################################# // code phase step (Code resampler phase increment per sample) [chips/sample] d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; if (trk_parameters.high_dyn) { - d_code_ph_history.push_back(std::pair(d_code_phase_step_chips, static_cast(d_current_prn_length_samples))); + d_code_ph_history.push_back(std::pair(d_code_phase_step_chips, static_cast(d_current_integration_length_samples))); if (d_code_ph_history.full()) { double tmp_cp1 = 0.0; @@ -808,11 +980,127 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() } } // remnant code phase [chips] - d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_prn_length_samples); // rounding error < 1 sample + d_rem_code_phase_samples_prev = d_rem_code_phase_samples; + d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_integration_length_samples); // rounding error < 1 sample d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / trk_parameters.fs_in; } +void dll_pll_veml_tracking_fpga::update_tracking_vars_extend_integration_in_FPGA() +{ + // first compute the long integration intermediate prn length values based on the current values (not the values that are compute here for the next iteration) + +// d_extended_integration_first_prn_length_samples = static_cast(std::floor(T_prn_samples + d_rem_code_phase_samples_prev)); +// d_extended_integration_next_prn_length_samples = static_cast(std::floor(T_prn_samples)); + + d_extended_integration_first_prn_length_samples = d_current_integration_length_samples - (trk_parameters.extend_correlation_symbols - 1)*static_cast(std::floor(T_prn_samples)); + d_extended_integration_next_prn_length_samples = static_cast(std::floor(T_prn_samples)); + + T_chip_seconds = 1.0 / d_code_freq_chips; + T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); + + // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + //T_prn_samples_prev = T_prn_samples; + T_prn_samples_prev = T_prn_samples; + T_prn_samples = T_prn_seconds * trk_parameters.fs_in; + K_blk_samples_prev = K_blk_samples; + K_blk_samples = T_prn_samples*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples; + + //d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); + d_actual_blk_length = static_cast(std::floor(K_blk_samples)); + d_next_integration_length_samples = 2*d_actual_blk_length - d_current_integration_length_samples; + + + //int32_t actual_prn_length_samples = static_cast(std::floor(K_blk_samples)); + //d_next_prn_length_samples = actual_prn_length_samples + (actual_prn_length_samples - d_current_prn_length_samples); + + + //################### PLL COMMANDS ################################################# + // carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in; + // carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2] + if (trk_parameters.high_dyn) + { + d_carr_ph_history.push_back(std::pair(d_carrier_phase_step_rad, static_cast(d_current_integration_length_samples))); + if (d_carr_ph_history.full()) + { + double tmp_cp1 = 0.0; + double tmp_cp2 = 0.0; + double tmp_samples = 0.0; + for (unsigned int k = 0; k < trk_parameters.smoother_length; k++) + { + tmp_cp1 += d_carr_ph_history[k].first; + tmp_cp2 += d_carr_ph_history[trk_parameters.smoother_length * 2 - k - 1].first; + tmp_samples += d_carr_ph_history[trk_parameters.smoother_length * 2 - k - 1].second; + } + tmp_cp1 /= static_cast(trk_parameters.smoother_length); + tmp_cp2 /= static_cast(trk_parameters.smoother_length); + d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples; + } + } + //std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; + // remnant carrier phase to prevent overflow in the code NCO + + // compute the long integration intermediate phase values + //d_extended_integration_first_rem_carr_phase_rad = d_rem_carr_phase_rad + static_cast(d_carrier_phase_step_rad * static_cast(d_extended_integration_first_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_extended_integration_first_prn_length_samples) * static_cast(d_extended_integration_first_prn_length_samples)); + //d_extended_integration_next_rem_carr_phase_rad_step= static_cast(d_carrier_phase_step_rad * static_cast(d_extended_integration_next_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_extended_integration_next_prn_length_samples) * static_cast(d_extended_integration_next_prn_length_samples));; + + d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); + + // debug + debug_d_rem_carr_phase_rad = d_rem_carr_phase_rad; + + d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); + + + + // carrier phase accumulator + //double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + //double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); + //std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl; + + + // compute the long integration intermediate phase values + d_extended_integration_first_acc_carrier_phase_rad = d_acc_carrier_phase_rad - (d_carrier_phase_step_rad * static_cast(d_extended_integration_first_prn_length_samples) - 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_extended_integration_first_prn_length_samples) * static_cast(d_extended_integration_first_prn_length_samples)); + d_extended_integration_next_acc_carrier_phase_rad_step = (d_carrier_phase_step_rad * static_cast(d_extended_integration_next_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_extended_integration_next_prn_length_samples) * static_cast(d_extended_integration_next_prn_length_samples)) + + 0.5 * 2 * d_carrier_phase_rate_step_rad * static_cast(d_extended_integration_next_prn_length_samples*d_extended_integration_first_prn_length_samples); + d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); + + //################### DLL COMMANDS ################################################# + // code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; + if (trk_parameters.high_dyn) + { + d_code_ph_history.push_back(std::pair(d_code_phase_step_chips, static_cast(d_current_integration_length_samples))); + if (d_code_ph_history.full()) + { + double tmp_cp1 = 0.0; + double tmp_cp2 = 0.0; + double tmp_samples = 0.0; + for (unsigned int k = 0; k < trk_parameters.smoother_length; k++) + { + tmp_cp1 += d_code_ph_history[k].first; + tmp_cp2 += d_code_ph_history[trk_parameters.smoother_length * 2 - k - 1].first; + tmp_samples += d_code_ph_history[trk_parameters.smoother_length * 2 - k - 1].second; + } + tmp_cp1 /= static_cast(trk_parameters.smoother_length); + tmp_cp2 /= static_cast(trk_parameters.smoother_length); + d_code_phase_rate_step_chips = (tmp_cp2 - tmp_cp1) / tmp_samples; + } + } + // remnant code phase [chips] + d_rem_code_phase_samples_prev = d_rem_code_phase_samples; + d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_integration_length_samples); // rounding error < 1 sample +// d_rem_code_phase_samples_first = K_blk_samples - static_cast(d_extended_integration_first_prn_length_samples); +// d_rem_code_phase_samples_next = K_blk_samples - static_cast(d_extended_integration_next_prn_length_samples); + d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / trk_parameters.fs_in; + + +} + + void dll_pll_veml_tracking_fpga::save_correlation_results() { if (d_secondary) @@ -867,6 +1155,67 @@ void dll_pll_veml_tracking_fpga::save_correlation_results() } } +void dll_pll_veml_tracking_fpga::save_correlation_results_extended_integration_in_FPGA() +{ +// if (d_secondary) +// { +// if (d_secondary_code_string->at(d_current_symbol) == '0') +// { +// if (d_veml) +// { +// d_VE_accu += *d_Very_Early; +// d_VL_accu += *d_Very_Late; +// } +// d_E_accu += *d_Early; +// d_P_accu += *d_Prompt; +// d_L_accu += *d_Late; +// } +// else +// { +// if (d_veml) +// { +// d_VE_accu -= *d_Very_Early; +// d_VL_accu -= *d_Very_Late; +// } +// d_E_accu -= *d_Early; +// d_P_accu -= *d_Prompt; +// d_L_accu -= *d_Late; +// } +// d_current_symbol++; +// // secondary code roll-up +// d_current_symbol %= d_secondary_code_length; +// } +// else +// { + if (d_veml) + { + d_VE_accu += *d_Very_Early; + d_VL_accu += *d_Very_Late; + } + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + +// if (trk_parameters.track_pilot) +// { +// d_P_data_accu += *d_Prompt_data; // accumulate pilot result too +// } + +// //d_current_symbol++; +// d_current_symbol = d_current_symbol + trk_parameters.extend_correlation_symbols; +// d_current_symbol %= d_symbols_per_bit; +// +// } + // If tracking pilot, disable Costas loop + if (trk_parameters.track_pilot) + { + d_cloop = false; + } + else + { + d_cloop = true; + } +} void dll_pll_veml_tracking_fpga::log_data(bool integrating) { @@ -945,7 +1294,8 @@ void dll_pll_veml_tracking_fpga::log_data(bool integrating) d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); // PRN start sample stamp - tmp_long_int = d_sample_counter + static_cast(d_current_prn_length_samples); + //tmp_long_int = d_sample_counter + static_cast(d_current_integration_length_samples); + tmp_long_int = d_sample_counter_next; d_dump_file.write(reinterpret_cast(&tmp_long_int), sizeof(uint64_t)); // accumulated carrier phase tmp_float = d_acc_carrier_phase_rad; @@ -980,7 +1330,7 @@ void dll_pll_veml_tracking_fpga::log_data(bool integrating) // AUX vars (for debug purposes) tmp_float = d_rem_code_phase_samples; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + tmp_double = static_cast(d_sample_counter + d_current_integration_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // PRN uint32_t prn_ = d_acquisition_gnss_synchro->PRN; @@ -994,6 +1344,165 @@ void dll_pll_veml_tracking_fpga::log_data(bool integrating) } +void dll_pll_veml_tracking_fpga::log_data_extended_integration_in_FPGA(bool integrating, bool extended_correlation_in_fpga_enabled) +{ + if (d_dump) + { + // Dump results to file + float prompt_I; + float prompt_Q; + float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; + float tmp_float; + double tmp_double; + uint64_t tmp_long_int; + if (trk_parameters.track_pilot) + { + if (interchange_iq) + { + prompt_I = d_Prompt_Data->imag(); + prompt_Q = d_Prompt_Data->real(); + } + else + { + prompt_I = d_Prompt_Data->real(); + prompt_Q = d_Prompt_Data->imag(); + } + } + else + { + if (interchange_iq) + { + prompt_I = d_Prompt->imag(); + prompt_Q = d_Prompt->real(); + } + else + { + prompt_I = d_Prompt->real(); + prompt_Q = d_Prompt->imag(); + } + } + if (d_veml) + { + tmp_VE = std::abs(d_VE_accu); + tmp_VL = std::abs(d_VL_accu); + } + else + { + tmp_VE = 0.0; + tmp_VL = 0.0; + } + tmp_E = std::abs(d_E_accu); + tmp_P = std::abs(d_P_accu); + tmp_L = std::abs(d_L_accu); +// if (integrating) +// { +// //TODO: Improve this solution! +// // It compensates the amplitude difference while integrating +// if (d_extend_correlation_symbols_count > 0) +// { +// float scale_factor = static_cast(trk_parameters.extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); +// tmp_VE *= scale_factor; +// tmp_E *= scale_factor; +// tmp_P *= scale_factor; +// tmp_L *= scale_factor; +// tmp_VL *= scale_factor; +// } +// } + + for (uint32_t k=0;k(&tmp_VE), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + + // PRN start sample stamp + if (!extended_correlation_in_fpga_enabled) + { + tmp_long_int = d_sample_counter_next; + } + else + { + tmp_long_int = d_sample_counter + d_extended_integration_first_prn_length_samples + d_extended_integration_next_prn_length_samples*k; + } + d_dump_file.write(reinterpret_cast(&tmp_long_int), sizeof(uint64_t)); + + // accumulated carrier phase + if (!extended_correlation_in_fpga_enabled) + { + tmp_float = d_acc_carrier_phase_rad; + + } + else + { + tmp_float = d_extended_integration_first_acc_carrier_phase_rad - d_extended_integration_next_acc_carrier_phase_rad_step*k; + } + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + + // carrier and code frequency + tmp_float = d_carrier_doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // carrier phase rate [Hz/s] + tmp_float = d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // code phase rate [chips/s^2] + tmp_float = d_code_phase_rate_step_chips * trk_parameters.fs_in * trk_parameters.fs_in; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // PLL commands + tmp_float = d_carr_phase_error_hz; + //tmp_float = d_carr_error_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carr_error_filt_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = d_code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // CN0 and carrier lock test + tmp_float = d_CN0_SNV_dB_Hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // AUX vars (for debug purposes) + tmp_float = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + + //tmp_double = static_cast(d_sample_counter + d_current_integration_length_samples); + if (!extended_correlation_in_fpga_enabled) + { + tmp_double = static_cast(d_sample_counter_next); + } + else + { + tmp_double = static_cast(d_sample_counter + d_extended_integration_first_prn_length_samples*(k+1)); + } + + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // PRN + uint32_t prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(uint32_t)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing trk dump file " << e.what(); + } + } + } +} + + + int32_t dll_pll_veml_tracking_fpga::save_matfile() { // READ DUMP FILE @@ -1242,7 +1751,69 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile() void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) { d_channel = channel; + + //debug + //if (d_channel != 1) + //{ + // //d_extend_correlation_symbols = 1; + // d_sc_remodulate_enabled = 0; + // d_extended_correlation_in_fpga = 0; + // + // //d_enable_extended_integration = 0; + // + //} + + multicorrelator_fpga->set_channel(d_channel); + + + + +// if (d_enable_extended_integration == true) +// { +// if (d_extended_correlation_in_fpga == true) +// { +// // we can not write the secondary codes to the FPGA in the class constructors because the FPGA driver is not opened until the set_channel command is +// // sent to the FPGA multicorrelator (because the device name is linked to the channel number that is assigned to the current class). +// // Now we can write the secondary codes that do not depend on the PRN number +// if (trk_parameters.system == 'G') +// { +// if (signal_type == "L5") +// { +// if (trk_parameters.track_pilot) +// { +// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); +// multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); +// multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); +// } +// else +// { +// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_data, 0); +// multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_data); +// } +// } +// } +// else if (trk_parameters.system == 'E') +// { +// if (signal_type == "5X") +// { +// if (trk_parameters.track_pilot) +// { +// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); +// multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); +// } +// else +// { +// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_data, 0); +// multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_data); +// } +// } +// } +// } +// } +// printf("================ now the secondary code stuff would be written =================="); + + LOG(INFO) << "Tracking Channel set to " << d_channel; // ############# ENABLE DATA FILE LOG ################# if (d_dump) @@ -1288,12 +1859,16 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_carr_ph_history.clear(); + + + if (systemName == "GPS" and signal_type == "L5") { if (trk_parameters.track_pilot) { d_Prompt_Data[0] = gr_complex(0.0, 0.0); } + } else if (systemName == "Galileo" and signal_type == "1B") { @@ -1307,8 +1882,16 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) if (trk_parameters.track_pilot) { d_secondary_code_string = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); + + // used by the FPGA + d_secondary_code_string_pilot = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); + + //multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + } + } std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); @@ -1350,18 +1933,116 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_cloop = true; d_Prompt_circular_buffer.clear(); + + d_extended_integration_first_prn_length_samples = static_cast(trk_parameters.vector_length); + d_extended_integration_next_prn_length_samples = d_extended_integration_first_prn_length_samples; + + //d_extended_integration_first_rem_carr_phase_rad = 0.0; + //d_extended_integration_next_rem_carr_phase_rad_step = 0.0; + d_extended_integration_first_acc_carrier_phase_rad = 0.0; + d_extended_integration_next_acc_carrier_phase_rad_step = 0.0; + + d_num_current_syncrho_repetitions = 1; + + // initialize K_blk_samples_prev, T_prn_samples_prev (in the HW the frame length is corrected in the next frame, not in the current frame, + // so we need the default parameters for the first frame + + T_chip_seconds = 1.0 / d_code_freq_chips; + T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); + T_prn_samples_prev = T_prn_seconds * trk_parameters.fs_in; + K_blk_samples_prev = T_prn_samples_prev*trk_parameters.extend_correlation_symbols; + + //multicorrelator_fpga->init_secondary_code_indices(); + + + d_debug_counter = 0; + + enable_post_apply_secondary_code = 0; + d_secondary_code_post_apply_counter = 0; + d_secondary_code_debug_counter_whole_bits = -1; + + + + // re-establish nominal integration length (not extended integration by default) + d_current_integration_length_samples = static_cast(trk_parameters.vector_length); + d_next_integration_length_samples = d_current_integration_length_samples; + + multicorrelator_fpga->disable_secondary_codes(); // make sure the processing of the secondary codes is disabled by default + // debug + //debug_first_time = 1; + + // updating the secondary code parameters is something that could be done only once during sw initialization. + // however, as the system works now, the SW resets the HW by launching a reset command from the acquisition class + // once for each channel during channel initialization. This erases the secondary length parameters that are already written + // to the hw during initialisation. That's why we need to do it here. + // in short a HW modification will be done in which the secondary code lengths nor the secondary codes are erased during reset + // in this way we will be able to program the secondary code parameters during initialisation and save some cpu cycles during + // real-time execution of gnss-sdr + if (d_enable_extended_integration == true) + { + if (d_extended_correlation_in_fpga == true) + { + // we can not write the secondary codes to the FPGA in the class constructors because the FPGA driver is not opened until the set_channel command is + // sent to the FPGA multicorrelator (because the device name is linked to the channel number that is assigned to the current class). + // Now we can write the secondary codes that do not depend on the PRN number + if (trk_parameters.system == 'G') + { + if (signal_type == "L5") + { + if (trk_parameters.track_pilot) + { + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); + multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); + } + else + { + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_data, 0); + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_data); + } + } + } + else if (trk_parameters.system == 'E') + { + if (signal_type == "5X") + { + if (trk_parameters.track_pilot) + { + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); + multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); + } + else + { + // the coherent integration in the fpga when using E5a without tracking the pilot cannot be done + // until the bug in the initialization variables related to the acquisition of the secondary code is + // solved in the SW + + //multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_data, 0); + //multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_data); + } + } + } + } + } + } + + + } void dll_pll_veml_tracking_fpga::stop_tracking() { + printf("stop tracking called\n"); d_state = 0; } void dll_pll_veml_tracking_fpga::reset(void) { + printf("reset and unlock channel called\n"); multicorrelator_fpga->unlock_channel(); } @@ -1374,7 +2055,15 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un auto **out = reinterpret_cast(&output_items[0]); Gnss_Synchro current_synchro_data = Gnss_Synchro(); - d_current_prn_length_samples = d_next_prn_length_samples; + + + //auto start = std::chrono::system_clock::now(); + + + //d_current_prn_length_samples = d_next_prn_length_samples; + d_past_integration_length_samples = d_current_integration_length_samples; + d_current_integration_length_samples = d_next_integration_length_samples; + if (d_pull_in_transitory == true) @@ -1387,6 +2076,16 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un } } } + //printf("entering general work"); + //std::cout << "d_state = " << d_state << std::endl; + + bool extended_correlation_in_fpga_enabled = false; + //bool extended_correlation_in_sw_enabled = false; + + //uint64_t reported_sample_counter; + + //d_flag_printout = false; + switch (d_state) { case 0: // Standby - Consume samples at full throttle, do nothing @@ -1426,6 +2125,21 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un absolute_samples_offset = static_cast(delta_trk_to_acq_prn_start_samples); } + + // debug L5 + //absolute_samples_offset = 56615418; + //absolute_samples_offset = 399729106; +// if (d_channel == 0) +// { + //absolute_samples_offset = 96654773; + //absolute_samples_offset = 194417058; +// } + + // debug E5a +// absolute_samples_offset = 363577168; + + std::cout << "TRACKING absolute_samples_offset = " << absolute_samples_offset << std::endl; + multicorrelator_fpga->set_initial_sample(absolute_samples_offset); d_absolute_samples_offset = absolute_samples_offset; d_sample_counter = absolute_samples_offset; @@ -1445,9 +2159,12 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_acq_code_phase_samples = absolute_samples_offset; //d_current_prn_length_samples = round(T_prn_mod_samples); - d_current_prn_length_samples = trk_parameters.vector_length; + //d_current_prn_length_samples = trk_parameters.vector_length; + d_current_integration_length_samples = trk_parameters.vector_length; + + //d_next_prn_length_samples = d_current_prn_length_samples; + d_next_integration_length_samples = d_current_integration_length_samples; - d_next_prn_length_samples = d_current_prn_length_samples; int32_t samples_offset = round(d_acq_code_phase_samples); d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); d_state = 2; @@ -1462,13 +2179,19 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; + + + + *out[0] = *d_acquisition_gnss_synchro; return 1; } case 2: // Wide tracking and symbol synchronization { d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_prn_length_samples); + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + uint64_t d_sample_counter_tmp = d_sample_counter; + uint64_t d_sample_counter_next_tmp = d_sample_counter_next; do_correlation_step(); @@ -1501,8 +2224,10 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(false); + d_num_current_syncrho_repetitions = 1; + log_data(false); + //printf("aa\n"); if (d_secondary) { // ####### SECONDARY CODE LOCK ##### @@ -1600,6 +2325,8 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un } } + //printf("ff\n"); + current_synchro_data.Tracking_sample_counter = d_sample_counter_next; current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; @@ -1619,11 +2346,74 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un if (d_enable_extended_integration) { + printf("EXTENDED INTEGRATION ENABLED\n"); + //printf("in state 3"); + //std::cout << "d_current_prn_length_samples = " << d_current_prn_length_samples << std::endl; + // UPDATE INTEGRATION TIME d_extend_correlation_symbols_count = 0; d_current_correlation_time_s = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); - d_state = 3; // next state is the extended correlator integrator + if (d_extended_correlation_in_fpga) + { +// if (trk_parameters.track_pilot) +// { +// d_P_data_accu = gr_complex(0.0, 0.0); +// } + //d_flag_printout = true; +// std::cout << "state 2 d_current_integration_length_samples = " << d_current_integration_length_samples << std::endl; +// std::cout << "state 2 d_sample_counter = " << d_sample_counter_tmp << std::endl; +// std::cout << "state 2 d_sample_counter_next = " << d_sample_counter_next_tmp << std::endl; + +// // recompute current_integration_length as if it was a long integration +// double K_blk_samples_tmp = T_prn_samples_prev*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples_prev; +// +// //d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); +// int32_t d_actual_blk_length_tmp = static_cast(std::floor(K_blk_samples_tmp)); +// std::cout << "d_actual_blk_length_tmp = " << d_actual_blk_length_tmp << std::endl; +// std::cout << "d_past_integration_length_samples = " << d_past_integration_length_samples << std::endl; +// int32_t d_current_integration_length_samples_tmp = 2*d_actual_blk_length_tmp - d_past_integration_length_samples; +// std::cout << "d_current_integration_length_samples_tmp = " << d_current_integration_length_samples_tmp << std::endl; +// std::cout << "d_current_integration_length_samples = " << d_current_integration_length_samples << std::endl; + + + //d_next_prn_length_samples = d_current_prn_length_samples*trk_parameters.extend_correlation_symbols; + + // correction on already computed parameters + //d_next_integration_length_samples = d_current_integration_length_samples*trk_parameters.extend_correlation_symbols; + K_blk_samples = T_prn_samples*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples_prev; + d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); + + + + +// std::cout << "state 2 modifying d_next_integration_length_samples = " << d_next_integration_length_samples << std::endl; + //d_actual_blk_length = static_cast(std::floor(K_blk_samples)); + //d_next_integration_length_samples = 2*d_actual_blk_length - d_current_integration_length_samples_tmp; + + +// if (trk_parameters.extend_correlation_symbols == d_symbols_per_bit) +// { +// d_state = 6; +// } +// else +// { +// d_state = 5; +// } + if (d_sc_demodulate_enabled) + { + multicorrelator_fpga->enable_secondary_codes(); + } + + d_state = 6; + //d_state = 7; + + } + else + { + d_state = 3; // next state is the extended correlator integrator + } + LOG(INFO) << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); @@ -1633,6 +2423,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // Set narrow taps delay values [chips] d_code_loop_filter.set_update_interval(d_current_correlation_time_s); d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz); + std::cout << " pll_bw_narrow_hz << " << trk_parameters.pll_bw_narrow_hz << " fll_bw_hz " << trk_parameters.fll_bw_hz << std::endl; d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order); if (d_veml) { @@ -1658,7 +2449,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un case 3: // coherent integration (correlation time extension) { d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_prn_length_samples); + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; @@ -1697,6 +2488,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); } } + current_synchro_data.Tracking_sample_counter = d_sample_counter_next; current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; @@ -1709,13 +2501,14 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_extend_correlation_symbols_count = 0; d_state = 4; } + d_num_current_syncrho_repetitions = 1; log_data(true); break; } case 4: // narrow tracking { d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_prn_length_samples); + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; @@ -1763,13 +2556,17 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); } } + + current_synchro_data.Tracking_sample_counter = d_sample_counter_next; current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.correlation_length_ms = d_correlation_length_ms; - // enable write dump file this cycle (valid DLL/PLL cycle) + + + d_num_current_syncrho_repetitions = 1; log_data(false); // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); @@ -1782,14 +2579,299 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_state = 3; // new coherent integration (correlation time extension) cycle } } + break; + } + + + case 6: // narrow tracking IN THE FPGA + { + + + + //printf("in state 6"); + //std::cout << "d_current_prn_length_samples = " << d_current_prn_length_samples; + + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + //d_flag_printout = true; +// std::cout << "state 6 d_current_integration_length_samples = " << d_current_integration_length_samples << std::endl; +// std::cout << "state 6 d_sample_counter = " << d_sample_counter << std::endl; +// std::cout << "state 6 d_sample_counter_next = " << d_sample_counter_next << std::endl; + + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + + extended_correlation_in_fpga_enabled = true; + + // this must be computed for the secondary prn code + if (d_secondary) + { + + d_first_length_secondary_code = d_current_integration_length_samples - (trk_parameters.extend_correlation_symbols - 1)*static_cast(std::floor(T_prn_samples)); + d_next_length_secondary_code = static_cast(std::floor(T_prn_samples)); + // debug +// std::cout << "d_first_length_secondary_code = " << d_first_length_secondary_code << std::endl; +// std::cout << "d_next_length_secondary_code = " << d_next_length_secondary_code << std::endl; + + multicorrelator_fpga->update_secondary_code_length(d_first_length_secondary_code, d_next_length_secondary_code); + } + // perform a correlation step + + //std::cout << "going to do trk : d_current_integration_length_samples = " << d_current_integration_length_samples << std::endl; + + do_correlation_step(); + +// for (uint32_t k = 0;kat(d_secondary_code_post_apply_counter) == '1') + { + d_sc_prompt_changed = true; + current_synchro_data.Prompt_I = - current_synchro_data.Prompt_I; + current_synchro_data.Prompt_Q = - current_synchro_data.Prompt_Q; + } + d_secondary_code_post_apply_counter = d_secondary_code_post_apply_counter + 1; + if (d_secondary_code_post_apply_counter == d_secondary_code_post_apply_length) + { + d_secondary_code_post_apply_counter = 0; + } + } + + if (d_flag_printout == true) + { + outfile.open("trk_out.txt", std::ios_base::app); + //outfile << "d_num_current_syncrho_repetitions = " << d_num_current_syncrho_repetitions << " d_debug_counter = " << d_debug_counter << std::endl; + outfile << d_debug_counter << " "; + //outfile << current_synchro_data.Prompt_I << " "; // << std::endl; + //outfile << current_synchro_data.Prompt_Q << " "; //<< std::endl; + if (current_synchro_data.Prompt_I < 0) + { + //outfile << "1" << std::endl; + outfile << "1 " << debug_d_rem_carr_phase_rad << " " << d_rem_carr_phase_rad << std::endl; + } + else + { + //outfile << "0" << std::endl; + outfile << "0 " << debug_d_rem_carr_phase_rad << " " << d_rem_carr_phase_rad << std::endl; + } + + d_secondary_code_debug_counter_whole_bits = d_secondary_code_debug_counter_whole_bits + 1; + if (d_secondary_code_debug_counter_whole_bits == 10) + { + d_secondary_code_debug_counter_whole_bits = 0; + outfile << "--" << std::endl; + } + + outfile.close(); + d_debug_counter++; + } + } + out[0][k]=current_synchro_data; + if (d_sc_prompt_changed == true) + { + // put back to normal + current_synchro_data.Prompt_I = - current_synchro_data.Prompt_I; + current_synchro_data.Prompt_Q = - current_synchro_data.Prompt_Q; + } + } + //return d_num_current_syncrho_repetitions; + } + + + return d_num_current_syncrho_repetitions; + + +// if (current_synchro_data.Flag_valid_symbol_output) +// { +// current_synchro_data.fs = static_cast(trk_parameters.fs_in); +// //current_synchro_data.Tracking_sample_counter = d_sample_counter_next; +// *out[0] = current_synchro_data; +// return 1; } return 0; } diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 0cb708323..34dcf1ab2 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -87,9 +87,12 @@ private: void do_correlation_step(void); void run_dll_pll(); void update_tracking_vars(); + void update_tracking_vars_extend_integration_in_FPGA(); void clear_tracking_vars(); void save_correlation_results(); + void save_correlation_results_extended_integration_in_FPGA(); void log_data(bool integrating); + void log_data_extended_integration_in_FPGA(bool integrating, bool extended_correlation_in_fpga_enabled); int32_t save_matfile(); //void run_state_2(Gnss_Synchro ¤t_synchro_data); @@ -148,6 +151,9 @@ private: gr_complex d_L_accu; gr_complex d_VL_accu; +// gr_complex d_P_data_accu; // when the extended integration is done in the FPGA we need to accumulate the pilot correlator results too + uint32_t d_num_current_syncrho_repetitions; + gr_complex *d_Prompt_Data; double d_code_phase_step_chips; @@ -158,6 +164,9 @@ private: boost::circular_buffer> d_carr_ph_history; // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; + double d_rem_code_phase_samples_first; + double d_rem_code_phase_samples_next; + double d_rem_code_phase_samples_prev; float d_rem_carr_phase_rad; // PLL and DLL filter library @@ -184,8 +193,13 @@ private: double T_prn_seconds; double T_prn_samples; double K_blk_samples; + + double K_blk_samples_prev; // PRN period in samples - int32_t d_current_prn_length_samples; + // REPLACED BY d_correlation_length_samples, d_next_integration_length_samples + //int32_t d_current_prn_length_samples; + int32_t d_current_integration_length_samples; + int32_t d_past_integration_length_samples; // processing samples counters uint64_t d_sample_counter; uint64_t d_acq_sample_stamp; @@ -204,6 +218,9 @@ private: gr_complex *d_Prompt_buffer; Exponential_Smoother d_cn0_smoother; + + bool d_extended_correlation_in_fpga; + // file dump std::ofstream d_dump_file; std::string d_dump_filename; @@ -212,8 +229,60 @@ private: // extra int32_t d_correlation_length_samples; - int32_t d_next_prn_length_samples; + //int32_t d_next_prn_length_samples; + int32_t d_next_integration_length_samples; + + int32_t d_extended_integration_first_prn_length_samples; + int32_t d_extended_integration_next_prn_length_samples; + + double d_extended_integration_first_acc_carrier_phase_rad; + double d_extended_integration_next_acc_carrier_phase_rad_step; + + //float d_extended_integration_first_rem_carr_phase_rad; + //float d_extended_integration_next_rem_carr_phase_rad_step; + uint64_t d_sample_counter_next; + + // DEBUG STUFF + uint64_t d_current_synchro_data_Tracking_sample_counter[20]; + double d_current_synchro_data_Code_phase_samples[20]; + double d_current_synchro_data_Carrier_phase_rads[20]; + double d_current_synchro_data_Carrier_Doppler_hz[20]; + double d_current_synchro_data_CN0_dB_hz[20]; + bool d_current_synchro_data_Flag_valid_symbol_output[20]; + int32_t d_current_synchro_data_correlation_length_ms[20]; + double d_current_synchro_data_Prompt_I[20]; + double d_current_synchro_data_Prompt_Q[20]; + + double T_prn_samples_prev; + int32_t d_actual_blk_length; + + bool d_flag_printout; + + std::string *d_secondary_code_string_data; + std::string *d_secondary_code_string_pilot; + + uint32_t d_secondary_code_length_data; + uint32_t d_secondary_code_length_pilot; + + uint32_t d_first_length_secondary_code; + uint32_t d_next_length_secondary_code; + + uint32_t d_debug_counter; + + uint32_t d_secondary_code_post_apply_counter; // init in set_gnss_synchro + std::string *d_secondary_code_string_post_apply; // init in constructor + uint32_t d_secondary_code_post_apply_length; // init in constructor + bool enable_post_apply_secondary_code; + uint32_t d_secondary_code_debug_counter_whole_bits; + bool d_sc_remodulate_enabled; + bool d_sc_demodulate_enabled; + bool d_sc_prompt_changed; + + float debug_d_rem_carr_phase_rad; + uint32_t debug_first_time; + + }; #endif //GNSS_SDR_DLL_PLL_VEML_TRACKING_FPGA_H diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc index 8ad8f3a13..3a4611989 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc @@ -77,4 +77,5 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() code_samples_per_chip = 0U; ca_codes = nullptr; data_codes = nullptr; + extended_correlation_in_fpga = false; } diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h index a614d32ff..ba5919a54 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h @@ -83,6 +83,7 @@ public: uint32_t code_samples_per_chip; int32_t* ca_codes; int32_t* data_codes; + bool extended_correlation_in_fpga; Dll_Pll_Conf_Fpga(); }; diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.cc b/src/algorithms/tracking/libs/fpga_multicorrelator.cc index 8299ba0ff..6a7c475a5 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.cc +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.cc @@ -119,6 +119,9 @@ Fpga_Multicorrelator_8sc::Fpga_Multicorrelator_8sc(int32_t n_correlators, d_code_samples_per_chip = code_samples_per_chip; d_code_length_samples = d_code_length_chips * d_code_samples_per_chip; + + d_secondary_code_enabled = false; + DLOG(INFO) << "TRACKING FPGA CLASS CREATED"; } @@ -206,6 +209,23 @@ void Fpga_Multicorrelator_8sc::Carrier_wipeoff_multicorrelator_resampler( std::cout << "Tracking_module Read failed to retrieve 4 bytes!" << std::endl; std::cout << "Tracking_module Interrupt number " << irq_count << std::endl; } + + // release secondary code indices, keep channel locked + if (d_secondary_code_enabled == true) + { + //printf("in the right place\n"); + // debug - force reset counter every time + //d_map_base[DROP_SAMPLES_REG_ADDR] = ENABLE_SECONDARY_CODE | INIT_SECONDARY_CODE_ADDRESSES; + d_map_base[DROP_SAMPLES_REG_ADDR] = ENABLE_SECONDARY_CODE; // keep secondary code enabled + +// //printf("do not enable secondary code on purpose\n"); +// d_map_base[DROP_SAMPLES_REG_ADDR] = 0; // block samples + } + else + { + //printf("in the wrong place\n"); + d_map_base[DROP_SAMPLES_REG_ADDR] = 0; // block samples + } Fpga_Multicorrelator_8sc::read_tracking_gps_results(); } @@ -457,8 +477,10 @@ void Fpga_Multicorrelator_8sc::read_tracking_gps_results(void) void Fpga_Multicorrelator_8sc::unlock_channel(void) { // unlock the channel to let the next samples go through - d_map_base[DROP_SAMPLES_REG_ADDR] = 1; // unlock the channel + d_map_base[DROP_SAMPLES_REG_ADDR] = DROP_SAMPLES; // unlock the channel and disable secondary codes d_map_base[STOP_TRACKING_REG_ADDR] = 1; // set the tracking module back to idle + + d_secondary_code_enabled = false; } @@ -478,3 +500,250 @@ void Fpga_Multicorrelator_8sc::lock_channel(void) // lock the channel for processing d_map_base[DROP_SAMPLES_REG_ADDR] = 0; // lock the channel } + + +void Fpga_Multicorrelator_8sc::set_secondary_code_lengths(uint32_t secondary_code_0_length, uint32_t secondary_code_1_length) +{ + + + + + + d_secondary_code_0_length = secondary_code_0_length; + d_secondary_code_1_length = secondary_code_1_length; + + // debug + //printf("warning extending the code length 0 to 20\n"); + //d_secondary_code_0_length = 20; + + uint32_t secondary_code_length_0_minus_1 = d_secondary_code_0_length - 1; + uint32_t secondary_code_length_1_minus_1 = d_secondary_code_1_length - 1; + + d_map_base[SECONDARY_CODE_LENGTHS_REG_ADDR] = secondary_code_length_1_minus_1*256 + secondary_code_length_0_minus_1; + + //std::cout << "setting secondary code lengths : \n"; + //std::cout << "initialized correlator 1 sec code length = " << d_secondary_code_1_length << " correlator 0 sec code length = " << d_secondary_code_0_length << std::endl; +} + +void Fpga_Multicorrelator_8sc::update_secondary_code_length(uint32_t first_length_secondary_code, uint32_t next_length_secondary_code) +{ + d_map_base[FIRST_PRN_LENGTH_MINUS_1_REG_ADDR] = first_length_secondary_code - 1; + d_map_base[NEXT_PRN_LENGTH_MINUS_1_REG_ADDR] = next_length_secondary_code - 1; + //std::cout << " first_length_secondary_code = " << first_length_secondary_code << " next_length_secondary_code = " << next_length_secondary_code << " sum = " << first_length_secondary_code + next_length_secondary_code << std::endl; + + + + +} + +void Fpga_Multicorrelator_8sc::initialize_secondary_code(uint32_t secondary_code, std::string *secondary_code_string) +{ + uint32_t secondary_code_length; + uint32_t reg_addr; + if (secondary_code == 0) + { + secondary_code_length = d_secondary_code_0_length; + reg_addr = PROG_SECONDARY_CODE_0_DATA_REG_ADDR; + + } + else + { + secondary_code_length = d_secondary_code_1_length; + reg_addr = PROG_SECONDARY_CODE_1_DATA_REG_ADDR; + } + Fpga_Multicorrelator_8sc::write_secondary_code(secondary_code_length, secondary_code_string, reg_addr); + +} + + +//void Fpga_Multicorrelator_8sc::initialize_secondary_codes(bool track_pilot, +// uint32_t secondary_code_length_data, std::string *secondary_code_string_data, +// uint32_t secondary_code_length_pilot, std::string *secondary_code_string_pilot) +//{ +// if (track_pilot) +// { +// // write secondary_code_length_pilot | secondary_code_length_data << 8 +// d_map_base[SECONDARY_CODE_LENGTHS_REG_ADDR] = secondary_code_length_pilot + secondary_code_length_data*256; +// +// // write pilot secondary code +// Fpga_Multicorrelator_8sc::write_secondary_code(secondary_code_length_pilot, secondary_code_string_pilot, PROG_SECONDARY_CODE_0_DATA_REG_ADDR); +// +// // write data secondary code +// Fpga_Multicorrelator_8sc::write_secondary_code(secondary_code_length_data, secondary_code_string_pilot, PROG_SECONDARY_CODE_1_DATA_REG_ADDR); +// +// } +// else +// { +// d_map_base[SECONDARY_CODE_LENGTHS_REG_ADDR] = secondary_code_length_data; +// +// // write data secondary code +// Fpga_Multicorrelator_8sc::write_secondary_code(secondary_code_length_data, secondary_code_string_pilot, PROG_SECONDARY_CODE_0_DATA_REG_ADDR); +// } +// +// std::cout << "going to print string " << std::endl; +// std::cout << secondary_code_string_data << std::endl; +// +// +//} + +void Fpga_Multicorrelator_8sc::write_secondary_code(uint32_t secondary_code_length, std::string *secondary_code_string, uint32_t reg_addr) +{ + uint32_t num_words = ceil(((float) secondary_code_length)/SECONDARY_CODE_WORD_SIZE); + uint32_t last_word_size = secondary_code_length % SECONDARY_CODE_WORD_SIZE; + //uint32_t initial_pointer; + + if (last_word_size == 0) + { + last_word_size = SECONDARY_CODE_WORD_SIZE; + } + // debug + //std::cout << "secondary_code_length = " << secondary_code_length << std::endl; + //std::cout << "secondary code string = " << *secondary_code_string << std::endl; + //std::cout << "reg_addr = " << reg_addr << std::endl; + + + // debug + //std::cout << "num_words = " << num_words << std::endl; + //std::cout << "last_word_size = " << last_word_size << std::endl; + + + uint32_t write_val = 0U; + uint32_t pow_k; + uint32_t mem_addr; + if (num_words > 1) + { + for (mem_addr = 0; mem_addr < num_words - 1 ;mem_addr++) + { + //std::cout << "------------------------------------------------------ going to write word " << mem_addr << std::endl; + write_val = 0U; + pow_k = 1; + for (unsigned int k=0;k>= 1; +// } +// printf("\n"); + } + } + write_val = 0U; + pow_k = 1; + mem_addr = num_words - 1; + + //std::cout << "------------------------------------------------------ going to write word " << mem_addr << std::endl; + + for (unsigned int k=0;k>= 1; +// } +// printf("\n"); + //printf("\n=============================================================================* END OF THIS\n"); +} + +//void Fpga_Multicorrelator_8sc::init_secondary_code_indices(void) +//{ +// d_map_base[DROP_SAMPLES_REG_ADDR] = 5; // leave channel unlocked, and init +//} + +void Fpga_Multicorrelator_8sc::enable_secondary_codes() +{ + d_map_base[DROP_SAMPLES_REG_ADDR] = INIT_SECONDARY_CODE_ADDRESSES | ENABLE_SECONDARY_CODE; // enable secondary codes and clear secondary code indices + d_secondary_code_enabled = true; + //std::cout << "enabling secondary codes d_map_base[DROP_SAMPLES_REG_ADDR] = " << (INIT_SECONDARY_CODE_ADDRESSES | ENABLE_SECONDARY_CODE) << std::endl; + +// // debug +// printf("do not enable secondary code on purpose\n"); +// d_map_base[DROP_SAMPLES_REG_ADDR] = 0; +} + +void Fpga_Multicorrelator_8sc::disable_secondary_codes() +{ + // this function is to be called before starting the tracking process in order to disable the secondary codes by default + //printf("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx disabling secondary codes in fpga\n"); + d_map_base[DROP_SAMPLES_REG_ADDR] = DROP_SAMPLES; +} + diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.h b/src/algorithms/tracking/libs/fpga_multicorrelator.h index b7ca95140..890a39289 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.h +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.h @@ -58,6 +58,11 @@ #define PHASE_STEP_RATE_REG_ADDR 22 #define STOP_TRACKING_REG_ADDR 23 #define INT_ON_RST_REG_ADDR 24 // cause interrupt on reset to prevent deadlock +#define SECONDARY_CODE_LENGTHS_REG_ADDR 25 +#define PROG_SECONDARY_CODE_0_DATA_REG_ADDR 26 +#define PROG_SECONDARY_CODE_1_DATA_REG_ADDR 27 +#define FIRST_PRN_LENGTH_MINUS_1_REG_ADDR 28 +#define NEXT_PRN_LENGTH_MINUS_1_REG_ADDR 29 #define START_FLAG_ADDR 30 // read-write addresses #define TEST_REG_ADDR 31 @@ -67,6 +72,13 @@ #define SAMPLE_COUNTER_REG_ADDR_LSW 13 #define SAMPLE_COUNTER_REG_ADDR_MSW 14 +// FPGA-related constants +#define SECONDARY_CODE_WORD_SIZE 20 // the secondary codes are written in to the FPGA in words of SECONDARY_CODE_WORD_SIZE bits +#define SECONDARY_CODE_WR_STROBE 0x800000 // write strobe position in the secondary code write register +#define SECONDARY_CODE_ADDR_BITS 0x100000 // memory address position in the secondary code write register +#define DROP_SAMPLES 1 // bit 0 of DROP_SAMPLES_REG_ADDR +#define ENABLE_SECONDARY_CODE 2 // bit 1 of DROP_SAMPLES_REG_ADDR +#define INIT_SECONDARY_CODE_ADDRESSES 4 // bit 2 of DROP_SAMPLES_REG_ADDR /*! * \brief Class that implements carrier wipe-off and correlators. @@ -93,6 +105,16 @@ public: uint64_t read_sample_counter(); void lock_channel(void); void unlock_channel(void); +// void initialize_secondary_codes(bool track_pilot, +// uint32_t secondary_code_length_data, std::string *secondary_code_string_data, +// uint32_t secondary_code_length_pilot, std::string *secondary_code_string_pilot); + void set_secondary_code_lengths(uint32_t secondary_code_0_length, uint32_t secondary_code_1_length); + void initialize_secondary_code(uint32_t secondary_code, std::string *secondary_code_string); + void update_secondary_code_length(uint32_t first_length_secondary_code, uint32_t next_length_secondary_code); + void enable_secondary_codes(); + void disable_secondary_codes(); +// void init_secondary_code_indices(); + private: gr_complex *d_corr_out; @@ -139,6 +161,10 @@ private: uint32_t d_multicorr_type; + uint32_t d_secondary_code_0_length; + uint32_t d_secondary_code_1_length; + + bool d_secondary_code_enabled; // private functions uint32_t fpga_acquisition_test_register(uint32_t writeval); void fpga_configure_tracking_gps_local_code(int32_t PRN); @@ -149,6 +175,7 @@ private: void fpga_launch_multicorrelator_fpga(void); void read_tracking_gps_results(void); void close_device(void); + void write_secondary_code(uint32_t secondary_code_length, std::string *secondary_code_string, uint32_t reg_addr); }; #endif /* GNSS_SDR_FPGA_MULTICORRELATOR_H_ */ From b48a70b9ddf34c94e3042566052d0bfdca593c59 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Tue, 18 Jun 2019 19:36:02 +0200 Subject: [PATCH 02/22] enabled FPGA extended coherent integration by default --- .../tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index 90cebf4a8..817cc3379 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -229,7 +229,7 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( trk_param_fpga.ca_codes = d_ca_codes; trk_param_fpga.code_length_chips = GPS_L1_CA_CODE_LENGTH_CHIPS; trk_param_fpga.code_samples_per_chip = 1; // 1 sample per chip - trk_param_fpga.extended_correlation_in_fpga = false; + trk_param_fpga.extended_correlation_in_fpga = true; //################# MAKE TRACKING GNURadio object ################### tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; From 0bb38ce38bd00a2dd35ff2aa65e59a4d47ba89f3 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Thu, 20 Jun 2019 14:28:47 +0200 Subject: [PATCH 03/22] removed some not used variables + code optimization --- .../gnuradio_blocks/pcps_acquisition_fpga.cc | 30 +- .../dll_pll_veml_tracking_fpga.cc | 533 ++++-------------- .../dll_pll_veml_tracking_fpga.h | 10 +- 3 files changed, 133 insertions(+), 440 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc index 17fbe2ca4..ccfc5de4b 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc @@ -199,21 +199,21 @@ void pcps_acquisition_fpga::send_positive_acquisition() << ", input signal power " << d_input_power; - std::cout << "positive acquisition" - << ", satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN - << ", sample_stamp " << d_sample_counter - << ", test statistics value " << d_test_statistics - << ", test statistics threshold " << d_threshold - << ", code phase " << d_gnss_synchro->Acq_delay_samples - << ", doppler " << d_gnss_synchro->Acq_doppler_hz - << ", magnitude " << d_mag - << ", input signal power " << d_input_power - << ", d_gnss_synchro->Acq_samplestamp_samples " << d_gnss_synchro->Acq_samplestamp_samples - << ", d_gnss_synchro->Flag_valid_word " << d_gnss_synchro->Flag_valid_word - << ", Flag_valid_pseudorange " << d_gnss_synchro->Flag_valid_pseudorange - << ", d_gnss_synchro->Flag_valid_symbol_output " << d_gnss_synchro->Flag_valid_symbol_output - << ", d_gnss_synchro->Flag_valid_acquisition " << d_gnss_synchro->Flag_valid_acquisition - << std::endl; +// std::cout << "positive acquisition" +// << ", satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN +// << ", sample_stamp " << d_sample_counter +// << ", test statistics value " << d_test_statistics +// << ", test statistics threshold " << d_threshold +// << ", code phase " << d_gnss_synchro->Acq_delay_samples +// << ", doppler " << d_gnss_synchro->Acq_doppler_hz +// << ", magnitude " << d_mag +// << ", input signal power " << d_input_power +// << ", d_gnss_synchro->Acq_samplestamp_samples " << d_gnss_synchro->Acq_samplestamp_samples +// << ", d_gnss_synchro->Flag_valid_word " << d_gnss_synchro->Flag_valid_word +// << ", Flag_valid_pseudorange " << d_gnss_synchro->Flag_valid_pseudorange +// << ", d_gnss_synchro->Flag_valid_symbol_output " << d_gnss_synchro->Flag_valid_symbol_output +// << ", d_gnss_synchro->Flag_valid_acquisition " << d_gnss_synchro->Flag_valid_acquisition +// << std::endl; //the channel FSM is set, so, notify it directly the positive acquisition to minimize delays d_channel_fsm.lock()->Event_valid_acquisition(); 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 66367aeaf..b2a664b2c 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 @@ -128,7 +128,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_extended_correlation_in_fpga = trk_parameters.extended_correlation_in_fpga; - printf("d_extended_correlation_in_fpga = %d\n", d_extended_correlation_in_fpga); + //printf("d_extended_correlation_in_fpga = %d\n", d_extended_correlation_in_fpga); d_sc_remodulate_enabled = false; // by default d_sc_demodulate_enabled = false; @@ -299,16 +299,6 @@ 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; -// if (d_extended_correlation_in_fpga == true) -// { -// if (trk_parameters.extend_correlation_symbols > 1) -// { -// //printf("EXTENDED CORRELATION IN FPGA ENABLED => ENABLING SECONDARY CODE REMODULATION\n"); -// d_sc_remodulate_enabled = true; -// } -// } - - if (trk_parameters.track_pilot) { @@ -316,7 +306,6 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & { if (trk_parameters.extend_correlation_symbols > 1) { - //printf("EXTENDED CORRELATION IN FPGA ENABLED => ENABLING SECONDARY CODE REMODULATION\n"); d_sc_demodulate_enabled = true; d_sc_remodulate_enabled = true; } @@ -345,33 +334,12 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & { - //d_flag_printout = true; - //Do not acquire secondary code in data component. It is done in telemetry decoder d_secondary = false; signal_pretty_name = signal_pretty_name + "I"; interchange_iq = false; - // the coherent integration in the fpga when using E5a without tracking the pilot cannot be done - // until the bug in the initialization variables related to the acquisition of the secondary code is - // solved in the SW - -// printf("################# SET EXTENDED CORRELATION TO ZERO\n"); -// d_extended_correlation_in_fpga == false; -// d_sc_demodulate_enabled = false; -// d_sc_remodulate_enabled = false; - -// if (d_extended_correlation_in_fpga == true) -// { -// d_extended_correlation_in_fpga == false; -// } - // used by the FPGA - //d_secondary_code_length_data = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); - //d_secondary_code_string_data = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); - - //d_secondary_code_string_post_apply = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); - //d_secondary_code_post_apply_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); - + // the coherent integration in the fpga when using E5a without tracking the pilot is not enabled } @@ -457,13 +425,13 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & if (trk_parameters.extend_correlation_symbols > 1) { d_enable_extended_integration = true; - printf("d_enable_extended_integration enabled\n"); + //printf("d_enable_extended_integration enabled\n"); } else { d_enable_extended_integration = false; trk_parameters.extend_correlation_symbols = 1; - printf("d_enable_extended_integration disabled\n"); + //printf("d_enable_extended_integration disabled\n"); } // --- Initializations --- @@ -773,7 +741,6 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra } if (d_carrier_lock_fail_counter > trk_parameters.max_lock_fail) { - printf("LOSS OF LOCK (TRACKING) --------------------------------------------x\n"); std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock @@ -897,32 +864,19 @@ void dll_pll_veml_tracking_fpga::clear_tracking_vars() void dll_pll_veml_tracking_fpga::update_tracking_vars() { -// // debug -- remove when done ! -// d_extended_integration_first_prn_length_samples = d_current_integration_length_samples - (trk_parameters.extend_correlation_symbols - 1)*static_cast(std::floor(T_prn_samples)); -// d_extended_integration_next_prn_length_samples = static_cast(std::floor(T_prn_samples)); -// // end of debug -- remove when done! - T_chip_seconds = 1.0 / d_code_freq_chips; // d_code_freq_chips updated in dll-pll T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); // d_code_freq_chips fixed permanently // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### // keep alignment parameters for the next input buffer // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - //T_prn_samples_prev = T_prn_samples; T_prn_samples_prev = T_prn_samples; T_prn_samples = T_prn_seconds * trk_parameters.fs_in; K_blk_samples_prev = K_blk_samples; K_blk_samples = T_prn_samples + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function - //d_next_prn_length_samples = static_cast(std::floor(K_blk_samples)); // round to a discrete number of samples - //d_next_prn_length_samples = static_cast(std::floor(K_blk_samples)); // round to a discrete number of samples d_actual_blk_length = static_cast(std::floor(K_blk_samples)); d_next_integration_length_samples = 2*d_actual_blk_length - d_current_integration_length_samples; - //d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); - - - //int32_t actual_prn_length_samples = static_cast(std::floor(K_blk_samples)); - //d_next_prn_length_samples = actual_prn_length_samples + (actual_prn_length_samples - d_current_prn_length_samples); //################### PLL COMMANDS ################################################# // carrier phase step (NCO phase increment per sample) [rads/sample] @@ -947,8 +901,7 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples; } } - //std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; - // remnant carrier phase to prevent overflow in the code NCO + // remnant carrier phase d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); // debug @@ -957,9 +910,6 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); // carrier phase accumulator - //double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); - //double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); - //std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl; d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); //################### DLL COMMANDS ################################################# @@ -995,9 +945,6 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars_extend_integration_in_FPGA { // first compute the long integration intermediate prn length values based on the current values (not the values that are compute here for the next iteration) -// d_extended_integration_first_prn_length_samples = static_cast(std::floor(T_prn_samples + d_rem_code_phase_samples_prev)); -// d_extended_integration_next_prn_length_samples = static_cast(std::floor(T_prn_samples)); - d_extended_integration_first_prn_length_samples = d_current_integration_length_samples - (trk_parameters.extend_correlation_symbols - 1)*static_cast(std::floor(T_prn_samples)); d_extended_integration_next_prn_length_samples = static_cast(std::floor(T_prn_samples)); @@ -1007,21 +954,14 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars_extend_integration_in_FPGA // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### // keep alignment parameters for the next input buffer // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - //T_prn_samples_prev = T_prn_samples; T_prn_samples_prev = T_prn_samples; T_prn_samples = T_prn_seconds * trk_parameters.fs_in; K_blk_samples_prev = K_blk_samples; K_blk_samples = T_prn_samples*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples; - //d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); d_actual_blk_length = static_cast(std::floor(K_blk_samples)); d_next_integration_length_samples = 2*d_actual_blk_length - d_current_integration_length_samples; - - //int32_t actual_prn_length_samples = static_cast(std::floor(K_blk_samples)); - //d_next_prn_length_samples = actual_prn_length_samples + (actual_prn_length_samples - d_current_prn_length_samples); - - //################### PLL COMMANDS ################################################# // carrier phase step (NCO phase increment per sample) [rads/sample] d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in; @@ -1045,13 +985,7 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars_extend_integration_in_FPGA d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples; } } - //std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; - // remnant carrier phase to prevent overflow in the code NCO - - // compute the long integration intermediate phase values - //d_extended_integration_first_rem_carr_phase_rad = d_rem_carr_phase_rad + static_cast(d_carrier_phase_step_rad * static_cast(d_extended_integration_first_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_extended_integration_first_prn_length_samples) * static_cast(d_extended_integration_first_prn_length_samples)); - //d_extended_integration_next_rem_carr_phase_rad_step= static_cast(d_carrier_phase_step_rad * static_cast(d_extended_integration_next_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_extended_integration_next_prn_length_samples) * static_cast(d_extended_integration_next_prn_length_samples));; - + // remnant carrier phase d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); // debug @@ -1059,14 +993,6 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars_extend_integration_in_FPGA d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); - - - // carrier phase accumulator - //double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); - //double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); - //std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl; - - // compute the long integration intermediate phase values d_extended_integration_first_acc_carrier_phase_rad = d_acc_carrier_phase_rad - (d_carrier_phase_step_rad * static_cast(d_extended_integration_first_prn_length_samples) - 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_extended_integration_first_prn_length_samples) * static_cast(d_extended_integration_first_prn_length_samples)); d_extended_integration_next_acc_carrier_phase_rad_step = (d_carrier_phase_step_rad * static_cast(d_extended_integration_next_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_extended_integration_next_prn_length_samples) * static_cast(d_extended_integration_next_prn_length_samples)) @@ -1098,8 +1024,6 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars_extend_integration_in_FPGA // remnant code phase [chips] d_rem_code_phase_samples_prev = d_rem_code_phase_samples; d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_integration_length_samples); // rounding error < 1 sample -// d_rem_code_phase_samples_first = K_blk_samples - static_cast(d_extended_integration_first_prn_length_samples); -// d_rem_code_phase_samples_next = K_blk_samples - static_cast(d_extended_integration_next_prn_length_samples); d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / trk_parameters.fs_in; @@ -1162,36 +1086,8 @@ void dll_pll_veml_tracking_fpga::save_correlation_results() void dll_pll_veml_tracking_fpga::save_correlation_results_extended_integration_in_FPGA() { -// if (d_secondary) -// { -// if (d_secondary_code_string->at(d_current_symbol) == '0') -// { -// if (d_veml) -// { -// d_VE_accu += *d_Very_Early; -// d_VL_accu += *d_Very_Late; -// } -// d_E_accu += *d_Early; -// d_P_accu += *d_Prompt; -// d_L_accu += *d_Late; -// } -// else -// { -// if (d_veml) -// { -// d_VE_accu -= *d_Very_Early; -// d_VL_accu -= *d_Very_Late; -// } -// d_E_accu -= *d_Early; -// d_P_accu -= *d_Prompt; -// d_L_accu -= *d_Late; -// } -// d_current_symbol++; -// // secondary code roll-up -// d_current_symbol %= d_secondary_code_length; -// } -// else -// { + // no need to remove the secondary code. When extended coherent integration is enabled + // the secondary code is always removed in the FPGA if (d_veml) { d_VE_accu += *d_Very_Early; @@ -1201,16 +1097,6 @@ void dll_pll_veml_tracking_fpga::save_correlation_results_extended_integration_i d_P_accu += *d_Prompt; d_L_accu += *d_Late; -// if (trk_parameters.track_pilot) -// { -// d_P_data_accu += *d_Prompt_data; // accumulate pilot result too -// } - -// //d_current_symbol++; -// d_current_symbol = d_current_symbol + trk_parameters.extend_correlation_symbols; -// d_current_symbol %= d_symbols_per_bit; -// -// } // If tracking pilot, disable Costas loop if (trk_parameters.track_pilot) { @@ -1399,20 +1285,9 @@ void dll_pll_veml_tracking_fpga::log_data_extended_integration_in_FPGA(bool inte tmp_E = std::abs(d_E_accu); tmp_P = std::abs(d_P_accu); tmp_L = std::abs(d_L_accu); -// if (integrating) -// { -// //TODO: Improve this solution! -// // It compensates the amplitude difference while integrating -// if (d_extend_correlation_symbols_count > 0) -// { -// float scale_factor = static_cast(trk_parameters.extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); -// tmp_VE *= scale_factor; -// tmp_E *= scale_factor; -// tmp_P *= scale_factor; -// tmp_L *= scale_factor; -// tmp_VL *= scale_factor; -// } -// } + + // no need to apply a scale factor. The + // the signal is scaled up by the longer integration time in the FPGA for (uint32_t k=0;k(&tmp_float), sizeof(float)); - - //tmp_double = static_cast(d_sample_counter + d_current_integration_length_samples); if (!extended_correlation_in_fpga_enabled) { tmp_double = static_cast(d_sample_counter_next); @@ -1492,7 +1365,6 @@ void dll_pll_veml_tracking_fpga::log_data_extended_integration_in_FPGA(bool inte { tmp_double = static_cast(d_sample_counter + d_extended_integration_first_prn_length_samples*(k+1)); } - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // PRN uint32_t prn_ = d_acquisition_gnss_synchro->PRN; @@ -1757,68 +1629,8 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) { d_channel = channel; - //debug - //if (d_channel != 1) - //{ - // //d_extend_correlation_symbols = 1; - // d_sc_remodulate_enabled = 0; - // d_extended_correlation_in_fpga = 0; - // - // //d_enable_extended_integration = 0; - // - //} - - multicorrelator_fpga->set_channel(d_channel); - - - -// if (d_enable_extended_integration == true) -// { -// if (d_extended_correlation_in_fpga == true) -// { -// // we can not write the secondary codes to the FPGA in the class constructors because the FPGA driver is not opened until the set_channel command is -// // sent to the FPGA multicorrelator (because the device name is linked to the channel number that is assigned to the current class). -// // Now we can write the secondary codes that do not depend on the PRN number -// if (trk_parameters.system == 'G') -// { -// if (signal_type == "L5") -// { -// if (trk_parameters.track_pilot) -// { -// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); -// multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); -// multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); -// } -// else -// { -// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_data, 0); -// multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_data); -// } -// } -// } -// else if (trk_parameters.system == 'E') -// { -// if (signal_type == "5X") -// { -// if (trk_parameters.track_pilot) -// { -// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); -// multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); -// } -// else -// { -// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_data, 0); -// multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_data); -// } -// } -// } -// } -// } -// printf("================ now the secondary code stuff would be written =================="); - - LOG(INFO) << "Tracking Channel set to " << d_channel; // ############# ENABLE DATA FILE LOG ################# if (d_dump) @@ -1833,8 +1645,6 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) { try { - //trk_parameters.dump_filename.append(boost::lexical_cast(d_channel)); - //trk_parameters.dump_filename.append(".dat"); d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); d_dump_file.open(dump_filename_.c_str(), std::ios::out | std::ios::binary); LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << dump_filename_.c_str(); @@ -1845,6 +1655,46 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) } } } + + if (d_enable_extended_integration == true) + { + if (d_extended_correlation_in_fpga == true) + { + // Now we can write the secondary codes that do not depend on the PRN number + if (trk_parameters.system == 'G') + { + if (signal_type == "L5") + { + if (trk_parameters.track_pilot) + { + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); + multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); + } + else + { + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_data, 0); + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_data); + } + } + } + else if (trk_parameters.system == 'E') + { + if (signal_type == "5X") + { + // coherent integration in the FPGA is only enabled when tracking the pilot. + if (trk_parameters.track_pilot) + { + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); + //multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); + multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); + } + + } + } + } + } + } @@ -1853,20 +1703,17 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_acquisition_gnss_synchro = p_gnss_synchro; if (p_gnss_synchro->PRN > 0) { - // When using the FPGA the SW only reads the sample counter during active tracking in order to spare CPU clock cycles. + // A set_gnss_synchro command with a valid PRN is received when the system is going to run acquisition with that PRN. + // We can use this command to pre-initialize tracking parameters and variables before the actual acquisition process takes place. + // In this way we minimize the latency between acquisition and tracking once the acquisition has been made. d_sample_counter = 0; d_sample_counter_next = 0; d_carrier_phase_rate_step_rad = 0.0; d_code_ph_history.clear(); - - d_carr_ph_history.clear(); - - - if (systemName == "GPS" and signal_type == "L5") { if (trk_parameters.track_pilot) @@ -1886,15 +1733,23 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) { if (trk_parameters.track_pilot) { + + d_secondary_code_string = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); - // used by the FPGA - d_secondary_code_string_pilot = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); - - //multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); - d_Prompt_Data[0] = gr_complex(0.0, 0.0); + if (d_enable_extended_integration == true) + { + if (d_extended_correlation_in_fpga == true) + { + // used by the FPGA + d_secondary_code_string_pilot = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); + + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); + + } + } } } @@ -1942,8 +1797,6 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_extended_integration_first_prn_length_samples = static_cast(trk_parameters.vector_length); d_extended_integration_next_prn_length_samples = d_extended_integration_first_prn_length_samples; - //d_extended_integration_first_rem_carr_phase_rad = 0.0; - //d_extended_integration_next_rem_carr_phase_rad_step = 0.0; d_extended_integration_first_acc_carrier_phase_rad = 0.0; d_extended_integration_next_acc_carrier_phase_rad_step = 0.0; @@ -1957,79 +1810,56 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) T_prn_samples_prev = T_prn_seconds * trk_parameters.fs_in; K_blk_samples_prev = T_prn_samples_prev*trk_parameters.extend_correlation_symbols; - //multicorrelator_fpga->init_secondary_code_indices(); - - d_debug_counter = 0; enable_post_apply_secondary_code = 0; d_secondary_code_post_apply_counter = 0; d_secondary_code_debug_counter_whole_bits = -1; - - // re-establish nominal integration length (not extended integration by default) d_current_integration_length_samples = static_cast(trk_parameters.vector_length); d_next_integration_length_samples = d_current_integration_length_samples; multicorrelator_fpga->disable_secondary_codes(); // make sure the processing of the secondary codes is disabled by default - // debug - //debug_first_time = 1; - // updating the secondary code parameters is something that could be done only once during sw initialization. - // however, as the system works now, the SW resets the HW by launching a reset command from the acquisition class - // once for each channel during channel initialization. This erases the secondary length parameters that are already written - // to the hw during initialisation. That's why we need to do it here. - // in short a HW modification will be done in which the secondary code lengths nor the secondary codes are erased during reset - // in this way we will be able to program the secondary code parameters during initialisation and save some cpu cycles during - // real-time execution of gnss-sdr - if (d_enable_extended_integration == true) - { - if (d_extended_correlation_in_fpga == true) - { - // we can not write the secondary codes to the FPGA in the class constructors because the FPGA driver is not opened until the set_channel command is - // sent to the FPGA multicorrelator (because the device name is linked to the channel number that is assigned to the current class). - // Now we can write the secondary codes that do not depend on the PRN number - if (trk_parameters.system == 'G') - { - if (signal_type == "L5") - { - if (trk_parameters.track_pilot) - { - multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); - multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); - multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); - } - else - { - multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_data, 0); - multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_data); - } - } - } - else if (trk_parameters.system == 'E') - { - if (signal_type == "5X") - { - if (trk_parameters.track_pilot) - { - multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); - multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); - multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); - } - else - { - // the coherent integration in the fpga when using E5a without tracking the pilot cannot be done - // until the bug in the initialization variables related to the acquisition of the secondary code is - // solved in the SW - - //multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_data, 0); - //multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_data); - } - } - } - } - } +// if (d_enable_extended_integration == true) +// { +// if (d_extended_correlation_in_fpga == true) +// { +// // Now we can write the secondary codes that do not depend on the PRN number +// if (trk_parameters.system == 'G') +// { +// if (signal_type == "L5") +// { +// if (trk_parameters.track_pilot) +// { +// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); +// multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); +// multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); +// } +// else +// { +// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_data, 0); +// multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_data); +// } +// } +// } +// else if (trk_parameters.system == 'E') +// { +// if (signal_type == "5X") +// { +// // coherent integration in the FPGA is only enabled when tracking the pilot. +// if (trk_parameters.track_pilot) +// { +// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); +// multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); +// multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); +// } +// +// } +// } +// } +// } } @@ -2060,17 +1890,8 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un auto **out = reinterpret_cast(&output_items[0]); Gnss_Synchro current_synchro_data = Gnss_Synchro(); - - - //auto start = std::chrono::system_clock::now(); - - - //d_current_prn_length_samples = d_next_prn_length_samples; - d_past_integration_length_samples = d_current_integration_length_samples; d_current_integration_length_samples = d_next_integration_length_samples; - - if (d_pull_in_transitory == true) { if (d_sample_counter > 0) // do not execute this condition until the sample counter has ben read for the first time after start_tracking @@ -2081,15 +1902,8 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un } } } - //printf("entering general work"); - //std::cout << "d_state = " << d_state << std::endl; bool extended_correlation_in_fpga_enabled = false; - //bool extended_correlation_in_sw_enabled = false; - - //uint64_t reported_sample_counter; - - //d_flag_printout = false; switch (d_state) { @@ -2131,19 +1945,8 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un absolute_samples_offset = static_cast(delta_trk_to_acq_prn_start_samples); } - // debug L5 - //absolute_samples_offset = 56615418; - //absolute_samples_offset = 399729106; -// if (d_channel == 0) -// { - //absolute_samples_offset = 96654773; - //absolute_samples_offset = 194417058; -// } - // debug E5a -// absolute_samples_offset = 363577168; - - std::cout << "TRACKING absolute_samples_offset = " << absolute_samples_offset << std::endl; + //std::cout << "TRACKING absolute_samples_offset = " << absolute_samples_offset << std::endl; multicorrelator_fpga->set_initial_sample(absolute_samples_offset); d_absolute_samples_offset = absolute_samples_offset; @@ -2157,17 +1960,11 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_code_freq_chips = radial_velocity * d_code_chip_rate; d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; d_code_phase_rate_step_chips = 0.0; - //double T_chip_mod_seconds = 1.0 / d_code_freq_chips; - //double T_prn_mod_seconds = T_chip_mod_seconds * static_cast(d_code_length_chips); - //double T_prn_mod_samples = T_prn_mod_seconds * trk_parameters.fs_in; d_acq_code_phase_samples = absolute_samples_offset; - //d_current_prn_length_samples = round(T_prn_mod_samples); - //d_current_prn_length_samples = trk_parameters.vector_length; d_current_integration_length_samples = trk_parameters.vector_length; - //d_next_prn_length_samples = d_current_prn_length_samples; d_next_integration_length_samples = d_current_integration_length_samples; int32_t samples_offset = round(d_acq_code_phase_samples); @@ -2184,10 +1981,6 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; - - - - *out[0] = *d_acquisition_gnss_synchro; return 1; } @@ -2221,18 +2014,17 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un else { bool next_state = false; - // Perform DLL/PLL tracking loop computations. Costas Loop enabled + // Perform DLL/PLL tracking loop computations. Costas Loop enabled run_dll_pll(); update_tracking_vars(); - // enable write dump file this cycle (valid DLL/PLL cycle) - d_num_current_syncrho_repetitions = 1; + // enable write dump file this cycle (valid DLL/PLL cycle) log_data(false); - //printf("aa\n"); + if (d_secondary) { // ####### SECONDARY CODE LOCK ##### @@ -2330,7 +2122,6 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un } } - //printf("ff\n"); current_synchro_data.Tracking_sample_counter = d_sample_counter_next; current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; @@ -2351,67 +2142,23 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un if (d_enable_extended_integration) { - printf("EXTENDED INTEGRATION ENABLED\n"); - //printf("in state 3"); - //std::cout << "d_current_prn_length_samples = " << d_current_prn_length_samples << std::endl; - - // UPDATE INTEGRATION TIME + // update integration time d_extend_correlation_symbols_count = 0; d_current_correlation_time_s = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); if (d_extended_correlation_in_fpga) { -// if (trk_parameters.track_pilot) -// { -// d_P_data_accu = gr_complex(0.0, 0.0); -// } - //d_flag_printout = true; -// std::cout << "state 2 d_current_integration_length_samples = " << d_current_integration_length_samples << std::endl; -// std::cout << "state 2 d_sample_counter = " << d_sample_counter_tmp << std::endl; -// std::cout << "state 2 d_sample_counter_next = " << d_sample_counter_next_tmp << std::endl; - -// // recompute current_integration_length as if it was a long integration -// double K_blk_samples_tmp = T_prn_samples_prev*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples_prev; -// -// //d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); -// int32_t d_actual_blk_length_tmp = static_cast(std::floor(K_blk_samples_tmp)); -// std::cout << "d_actual_blk_length_tmp = " << d_actual_blk_length_tmp << std::endl; -// std::cout << "d_past_integration_length_samples = " << d_past_integration_length_samples << std::endl; -// int32_t d_current_integration_length_samples_tmp = 2*d_actual_blk_length_tmp - d_past_integration_length_samples; -// std::cout << "d_current_integration_length_samples_tmp = " << d_current_integration_length_samples_tmp << std::endl; -// std::cout << "d_current_integration_length_samples = " << d_current_integration_length_samples << std::endl; - - - //d_next_prn_length_samples = d_current_prn_length_samples*trk_parameters.extend_correlation_symbols; // correction on already computed parameters - //d_next_integration_length_samples = d_current_integration_length_samples*trk_parameters.extend_correlation_symbols; K_blk_samples = T_prn_samples*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples_prev; d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); - - - -// std::cout << "state 2 modifying d_next_integration_length_samples = " << d_next_integration_length_samples << std::endl; - //d_actual_blk_length = static_cast(std::floor(K_blk_samples)); - //d_next_integration_length_samples = 2*d_actual_blk_length - d_current_integration_length_samples_tmp; - - -// if (trk_parameters.extend_correlation_symbols == d_symbols_per_bit) -// { -// d_state = 6; -// } -// else -// { -// d_state = 5; -// } if (d_sc_demodulate_enabled) { multicorrelator_fpga->enable_secondary_codes(); } - d_state = 6; - //d_state = 7; + d_state = 5; } else @@ -2428,7 +2175,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // Set narrow taps delay values [chips] d_code_loop_filter.set_update_interval(d_current_correlation_time_s); d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz); - std::cout << " pll_bw_narrow_hz << " << trk_parameters.pll_bw_narrow_hz << " fll_bw_hz " << trk_parameters.fll_bw_hz << std::endl; + //std::cout << " pll_bw_narrow_hz << " << trk_parameters.pll_bw_narrow_hz << " fll_bw_hz " << trk_parameters.fll_bw_hz << std::endl; d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order); if (d_veml) { @@ -2588,22 +2335,12 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un } - case 6: // narrow tracking IN THE FPGA + case 5: // narrow tracking IN THE FPGA { - - - //printf("in state 6"); - //std::cout << "d_current_prn_length_samples = " << d_current_prn_length_samples; - d_sample_counter = d_sample_counter_next; d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - //d_flag_printout = true; -// std::cout << "state 6 d_current_integration_length_samples = " << d_current_integration_length_samples << std::endl; -// std::cout << "state 6 d_sample_counter = " << d_sample_counter << std::endl; -// std::cout << "state 6 d_sample_counter_next = " << d_sample_counter_next << std::endl; - // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; @@ -2615,25 +2352,13 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_first_length_secondary_code = d_current_integration_length_samples - (trk_parameters.extend_correlation_symbols - 1)*static_cast(std::floor(T_prn_samples)); d_next_length_secondary_code = static_cast(std::floor(T_prn_samples)); - // debug -// std::cout << "d_first_length_secondary_code = " << d_first_length_secondary_code << std::endl; -// std::cout << "d_next_length_secondary_code = " << d_next_length_secondary_code << std::endl; multicorrelator_fpga->update_secondary_code_length(d_first_length_secondary_code, d_next_length_secondary_code); } + // perform a correlation step - - //std::cout << "going to do trk : d_current_integration_length_samples = " << d_current_integration_length_samples << std::endl; - do_correlation_step(); -// for (uint32_t k = 0;k(trk_parameters.fs_in); -// //current_synchro_data.Tracking_sample_counter = d_sample_counter_next; -// *out[0] = current_synchro_data; -// return 1; } return 0; } diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 34dcf1ab2..3ccace08f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -195,11 +195,9 @@ private: double K_blk_samples; double K_blk_samples_prev; - // PRN period in samples - // REPLACED BY d_correlation_length_samples, d_next_integration_length_samples - //int32_t d_current_prn_length_samples; + int32_t d_current_integration_length_samples; - int32_t d_past_integration_length_samples; + // processing samples counters uint64_t d_sample_counter; uint64_t d_acq_sample_stamp; @@ -229,7 +227,6 @@ private: // extra int32_t d_correlation_length_samples; - //int32_t d_next_prn_length_samples; int32_t d_next_integration_length_samples; int32_t d_extended_integration_first_prn_length_samples; @@ -238,9 +235,6 @@ private: double d_extended_integration_first_acc_carrier_phase_rad; double d_extended_integration_next_acc_carrier_phase_rad_step; - //float d_extended_integration_first_rem_carr_phase_rad; - //float d_extended_integration_next_rem_carr_phase_rad_step; - uint64_t d_sample_counter_next; // DEBUG STUFF From 2982961e499091ae5fe4022a70b54135c1a21bf0 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Fri, 28 Jun 2019 10:54:19 +0200 Subject: [PATCH 04/22] currently making changes to be able to extend the FPGA coherent integration beyond 20 ms for GPS L1, 1 ms for Galileo E1, 10 ms for GPS L5 and 20 ms for Galileo E5a using SW. --- .../galileo_e1_dll_pll_veml_tracking_fpga.cc | 2 + .../galileo_e5a_dll_pll_tracking_fpga.cc | 33 +++- .../gps_l1_ca_dll_pll_tracking_fpga.cc | 33 +++- .../adapters/gps_l5_dll_pll_tracking_fpga.cc | 32 +++- .../dll_pll_veml_tracking_fpga.cc | 163 ++++++++++++------ .../dll_pll_veml_tracking_fpga.h | 6 +- .../tracking/libs/dll_pll_conf_fpga.cc | 2 + .../tracking/libs/dll_pll_conf_fpga.h | 2 + 8 files changed, 212 insertions(+), 61 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc index 94619adb1..a199d2679 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc @@ -281,6 +281,8 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( trk_param_fpga.code_length_chips = GALILEO_E1_B_CODE_LENGTH_CHIPS; trk_param_fpga.code_samples_per_chip = code_samples_per_chip; // 2 sample per chip trk_param_fpga.extended_correlation_in_fpga = false; + trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) + trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) //################# MAKE TRACKING GNURadio object ################### tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index b7749a45b..2fa514fbf 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -262,14 +262,37 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( trk_param_fpga.data_codes = d_data_codes; trk_param_fpga.code_length_chips = code_length_chips; trk_param_fpga.code_samples_per_chip = code_samples_per_chip; // 2 sample per chip + + trk_param_fpga.extended_correlation_in_fpga = false; // by default + trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) + trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) if (d_track_pilot) { - trk_param_fpga.extended_correlation_in_fpga = true; - } - else - { - trk_param_fpga.extended_correlation_in_fpga = false; + if (extend_correlation_symbols >1) + { + if (extend_correlation_symbols <= GALILEO_E5A_I_SECONDARY_CODE_LENGTH) + { + if ((GALILEO_E5A_I_SECONDARY_CODE_LENGTH % extend_correlation_symbols) == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.fpga_integration_period = extend_correlation_symbols; + printf("correlation in fpga true\n"); + } + } + else + { + if (extend_correlation_symbols % GALILEO_E5A_I_SECONDARY_CODE_LENGTH == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.extend_fpga_integration_periods = extend_correlation_symbols/GALILEO_E5A_I_SECONDARY_CODE_LENGTH; + trk_param_fpga.fpga_integration_period = GALILEO_E5A_I_SECONDARY_CODE_LENGTH; + printf("correlation in fpga true\n"); + printf("extend fpga integration periods true\n"); + } + } + } } + //################# MAKE TRACKING GNURadio object ################### tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index 817cc3379..7a816f331 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -49,7 +49,8 @@ #include #define NUM_PRNs 32 // total number of PRNs - +#define GPS_CA_BIT_DURATION_MS 20 +#define GPS_CA_CODE_PERIOD_MS 1 // the following flag is FPGA-specific and they are using arrange the values of the local code in the way the FPGA // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. #define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000 // flag that enables WE (Write Enable) of the local code FPGA @@ -229,7 +230,35 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( trk_param_fpga.ca_codes = d_ca_codes; trk_param_fpga.code_length_chips = GPS_L1_CA_CODE_LENGTH_CHIPS; trk_param_fpga.code_samples_per_chip = 1; // 1 sample per chip - trk_param_fpga.extended_correlation_in_fpga = true; + + trk_param_fpga.extended_correlation_in_fpga = false; // by default + trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) + trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) + if (symbols_extended_correlator >1) + { + if (symbols_extended_correlator <= GPS_CA_BIT_DURATION_MS) + { + if ((GPS_CA_BIT_DURATION_MS % symbols_extended_correlator) == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.fpga_integration_period = symbols_extended_correlator; + printf("correlation in fpga true\n"); + } + } + else + { + if (symbols_extended_correlator % GPS_CA_BIT_DURATION_MS == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.extend_fpga_integration_periods = symbols_extended_correlator/GPS_CA_BIT_DURATION_MS; + trk_param_fpga.fpga_integration_period = GPS_CA_BIT_DURATION_MS; + printf("correlation in fpga true\n"); + printf("extend fpga integration periods true\n"); + } + } + } + + //################# MAKE TRACKING GNURadio object ################### tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index d94e96443..69b112bda 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -286,7 +286,37 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( trk_param_fpga.data_codes = d_data_codes; trk_param_fpga.code_length_chips = code_length_chips; trk_param_fpga.code_samples_per_chip = code_samples_per_chip; // 2 sample per chip - trk_param_fpga.extended_correlation_in_fpga = true; + + trk_param_fpga.extended_correlation_in_fpga = false; // by default + trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) + trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) + if (d_track_pilot) + { + if (extend_correlation_symbols > 1) + { + if (extend_correlation_symbols <= GPS_L5I_NH_CODE_LENGTH) + { + if ((GPS_L5I_NH_CODE_LENGTH % extend_correlation_symbols) == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.fpga_integration_period = extend_correlation_symbols; + printf("correlation in fpga true\n"); + } + } + else + { + if (extend_correlation_symbols % GPS_L5I_NH_CODE_LENGTH == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.extend_fpga_integration_periods = extend_correlation_symbols/GPS_L5I_NH_CODE_LENGTH; + trk_param_fpga.fpga_integration_period = GPS_L5I_NH_CODE_LENGTH; + printf("correlation in fpga true\n"); + printf("extend fpga integration periods true\n"); + } + } + } + } + tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; DLOG(INFO) << "tracking(" << tracking_fpga_sc->unique_id() << ")"; 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 b2a664b2c..8d0e2b0c9 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 @@ -128,6 +128,10 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_extended_correlation_in_fpga = trk_parameters.extended_correlation_in_fpga; + d_extend_fpga_integration_periods = trk_parameters.extend_fpga_integration_periods; // by default + + d_fpga_integration_period = trk_parameters.fpga_integration_period; // by default + //printf("d_extended_correlation_in_fpga = %d\n", d_extended_correlation_in_fpga); d_sc_remodulate_enabled = false; // by default @@ -204,6 +208,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & //printf("EXTENDED CORRELATION IN FPGA ENABLED => ENABLING SECONDARY CODE REMODULATION\n"); d_sc_demodulate_enabled = true; d_sc_remodulate_enabled = true; + } } @@ -872,7 +877,7 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation T_prn_samples_prev = T_prn_samples; T_prn_samples = T_prn_seconds * trk_parameters.fs_in; - K_blk_samples_prev = K_blk_samples; + //K_blk_samples_prev = K_blk_samples; K_blk_samples = T_prn_samples + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function d_actual_blk_length = static_cast(std::floor(K_blk_samples)); @@ -945,8 +950,10 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars_extend_integration_in_FPGA { // first compute the long integration intermediate prn length values based on the current values (not the values that are compute here for the next iteration) - d_extended_integration_first_prn_length_samples = d_current_integration_length_samples - (trk_parameters.extend_correlation_symbols - 1)*static_cast(std::floor(T_prn_samples)); - d_extended_integration_next_prn_length_samples = static_cast(std::floor(T_prn_samples)); + //d_extended_integration_first_prn_length_samples = d_current_integration_length_samples - (trk_parameters.extend_correlation_symbols - 1)*static_cast(std::floor(T_prn_samples)); + d_extended_integration_first_prn_length_samples = d_current_integration_length_samples - (d_fpga_integration_period - 1)*static_cast(std::floor(T_prn_samples)); + + d_extended_integration_next_prn_length_samples = static_cast(std::floor(T_prn_samples)); T_chip_seconds = 1.0 / d_code_freq_chips; T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); @@ -956,8 +963,9 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars_extend_integration_in_FPGA // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation T_prn_samples_prev = T_prn_samples; T_prn_samples = T_prn_seconds * trk_parameters.fs_in; - K_blk_samples_prev = K_blk_samples; - K_blk_samples = T_prn_samples*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples; + //K_blk_samples_prev = K_blk_samples; + //K_blk_samples = T_prn_samples*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples; + K_blk_samples = T_prn_samples*d_fpga_integration_period + d_rem_code_phase_samples; d_actual_blk_length = static_cast(std::floor(K_blk_samples)); d_next_integration_length_samples = 2*d_actual_blk_length - d_current_integration_length_samples; @@ -1808,8 +1816,9 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) T_chip_seconds = 1.0 / d_code_freq_chips; T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); T_prn_samples_prev = T_prn_seconds * trk_parameters.fs_in; - K_blk_samples_prev = T_prn_samples_prev*trk_parameters.extend_correlation_symbols; + //K_blk_samples_prev = T_prn_samples_prev*trk_parameters.extend_correlation_symbols; + //K_blk_samples_prev = T_prn_samples_prev*d_fpga_integration_period; d_debug_counter = 0; enable_post_apply_secondary_code = 0; @@ -1822,44 +1831,6 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) multicorrelator_fpga->disable_secondary_codes(); // make sure the processing of the secondary codes is disabled by default -// if (d_enable_extended_integration == true) -// { -// if (d_extended_correlation_in_fpga == true) -// { -// // Now we can write the secondary codes that do not depend on the PRN number -// if (trk_parameters.system == 'G') -// { -// if (signal_type == "L5") -// { -// if (trk_parameters.track_pilot) -// { -// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); -// multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); -// multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); -// } -// else -// { -// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_data, 0); -// multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_data); -// } -// } -// } -// else if (trk_parameters.system == 'E') -// { -// if (signal_type == "5X") -// { -// // coherent integration in the FPGA is only enabled when tracking the pilot. -// if (trk_parameters.track_pilot) -// { -// multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); -// multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); -// multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); -// } -// -// } -// } -// } -// } } @@ -2149,17 +2120,29 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un if (d_extended_correlation_in_fpga) { - // correction on already computed parameters - K_blk_samples = T_prn_samples*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples_prev; - d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); + if (d_sc_demodulate_enabled) { multicorrelator_fpga->enable_secondary_codes(); } - d_state = 5; + if (d_extend_fpga_integration_periods > 1) + { + // correction on already computed parameters + K_blk_samples = T_prn_samples*(trk_parameters.extend_correlation_symbols/d_extend_fpga_integration_periods) + d_rem_code_phase_samples_prev; + d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); + d_state = 5; + } + else + { + // correction on already computed parameters + K_blk_samples = T_prn_samples*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples_prev; + d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); + + d_state = 6; + } } else { @@ -2331,11 +2314,87 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_state = 3; // new coherent integration (correlation time extension) cycle } } - break; + break; } + case 5: // coherent integration (correlation time extension) + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - case 5: // narrow tracking IN THE FPGA + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + + extended_correlation_in_fpga_enabled = true; + + // this must be computed for the secondary prn code + if (d_secondary) + { + + d_first_length_secondary_code = d_current_integration_length_samples - (trk_parameters.extend_correlation_symbols - 1)*static_cast(std::floor(T_prn_samples)); + d_next_length_secondary_code = static_cast(std::floor(T_prn_samples)); + + multicorrelator_fpga->update_secondary_code_length(d_first_length_secondary_code, d_next_length_secondary_code); + } + + // perform a correlation step + do_correlation_step(); + update_tracking_vars_extend_integration_in_FPGA(); + save_correlation_results_extended_integration_in_FPGA(); + + // ########### Output the tracking results to Telemetry block ########## + if (interchange_iq) + { + if (trk_parameters.track_pilot) + { + // Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); + } + } + else + { + if (trk_parameters.track_pilot) + { + // Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + } + } + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_extended_integration_first_prn_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_extended_integration_first_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + + d_num_current_syncrho_repetitions = trk_parameters.extend_correlation_symbols; + + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count == (trk_parameters.extend_correlation_symbols - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 6; + } + + + log_data_extended_integration_in_FPGA(false, true); + break; + } + + + case 6: // narrow tracking IN THE FPGA { d_sample_counter = d_sample_counter_next; @@ -2443,7 +2502,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // debug d_sc_prompt_changed = false; - if (d_state == 3 || d_state == 4 | d_state ==5) + if (d_state == 3 || d_state == 4 | d_state ==6) { // debug - remodulate secondary code if (d_sc_remodulate_enabled == true) @@ -2519,7 +2578,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un current_synchro_data.Carrier_phase_rads = d_extended_integration_first_acc_carrier_phase_rad; d_sc_prompt_changed = false; - if (d_state == 3 || d_state == 4 | d_state ==5) + if (d_state == 3 || d_state == 4 | d_state ==6) { if (d_sc_remodulate_enabled == true) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 3ccace08f..40318a84f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -194,7 +194,7 @@ private: double T_prn_samples; double K_blk_samples; - double K_blk_samples_prev; + //double K_blk_samples_prev; int32_t d_current_integration_length_samples; @@ -276,6 +276,10 @@ private: float debug_d_rem_carr_phase_rad; uint32_t debug_first_time; + uint32_t d_extend_fpga_integration_periods; + + uint32_t d_fpga_integration_period; + }; diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc index 3a4611989..88c6ead1d 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc @@ -78,4 +78,6 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() ca_codes = nullptr; data_codes = nullptr; extended_correlation_in_fpga = false; + extend_fpga_integration_periods = 1; + fpga_integration_period = 0; } diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h index ba5919a54..c61066e84 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h @@ -84,6 +84,8 @@ public: int32_t* ca_codes; int32_t* data_codes; bool extended_correlation_in_fpga; + uint32_t extend_fpga_integration_periods; + uint32_t fpga_integration_period; Dll_Pll_Conf_Fpga(); }; From feccc62c04b57e27392594b457a81f09cf86ceb6 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Thu, 4 Jul 2019 12:48:35 +0200 Subject: [PATCH 05/22] solved some bugs related to the extended integration in the FPGA --- .../gps_l1_ca_dll_pll_tracking_fpga.cc | 15 +------ .../dll_pll_veml_tracking_fpga.cc | 42 ++++++++++++++----- 2 files changed, 34 insertions(+), 23 deletions(-) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index 7a816f331..2090d2b42 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -155,9 +155,9 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( symbols_extended_correlator = 1; std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be bigger than 1. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << std::endl; } - else if (symbols_extended_correlator > 20) + else if (symbols_extended_correlator > GPS_CA_BIT_DURATION_MS) { - symbols_extended_correlator = 20; + symbols_extended_correlator = GPS_CA_BIT_DURATION_MS; std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be lower than 21. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << std::endl; } trk_param_fpga.extend_correlation_symbols = symbols_extended_correlator; @@ -245,17 +245,6 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( printf("correlation in fpga true\n"); } } - else - { - if (symbols_extended_correlator % GPS_CA_BIT_DURATION_MS == 0) - { - trk_param_fpga.extended_correlation_in_fpga = true; - trk_param_fpga.extend_fpga_integration_periods = symbols_extended_correlator/GPS_CA_BIT_DURATION_MS; - trk_param_fpga.fpga_integration_period = GPS_CA_BIT_DURATION_MS; - printf("correlation in fpga true\n"); - printf("extend fpga integration periods true\n"); - } - } } 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 8d0e2b0c9..5241b8673 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 @@ -1296,6 +1296,23 @@ void dll_pll_veml_tracking_fpga::log_data_extended_integration_in_FPGA(bool inte // no need to apply a scale factor. The // the signal is scaled up by the longer integration time in the FPGA + //if (integrating) + if (d_extend_fpga_integration_periods > 1) + { + //TODO: Improve this solution! + // It compensates the amplitude difference while integrating + if (d_extend_correlation_symbols_count > 0) + { + float scale_factor = static_cast(trk_parameters.extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); + tmp_VE *= scale_factor; + tmp_E *= scale_factor; + tmp_P *= scale_factor; + tmp_L *= scale_factor; + tmp_VL *= scale_factor; + } + } + + for (uint32_t k=0;k 1) { // correction on already computed parameters - K_blk_samples = T_prn_samples*(trk_parameters.extend_correlation_symbols/d_extend_fpga_integration_periods) + d_rem_code_phase_samples_prev; + K_blk_samples = T_prn_samples*(d_fpga_integration_period) + d_rem_code_phase_samples_prev; d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); d_state = 5; @@ -2331,7 +2351,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un if (d_secondary) { - d_first_length_secondary_code = d_current_integration_length_samples - (trk_parameters.extend_correlation_symbols - 1)*static_cast(std::floor(T_prn_samples)); + d_first_length_secondary_code = d_current_integration_length_samples - (d_fpga_integration_period - 1)*static_cast(std::floor(T_prn_samples)); d_next_length_secondary_code = static_cast(std::floor(T_prn_samples)); multicorrelator_fpga->update_secondary_code_length(d_first_length_secondary_code, d_next_length_secondary_code); @@ -2379,10 +2399,10 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.correlation_length_ms = d_correlation_length_ms; - d_num_current_syncrho_repetitions = trk_parameters.extend_correlation_symbols; + d_num_current_syncrho_repetitions = d_fpga_integration_period; d_extend_correlation_symbols_count++; - if (d_extend_correlation_symbols_count == (trk_parameters.extend_correlation_symbols - 1)) + if (d_extend_correlation_symbols_count == (d_extend_fpga_integration_periods - 1)) { d_extend_correlation_symbols_count = 0; d_state = 6; @@ -2396,7 +2416,6 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un case 6: // narrow tracking IN THE FPGA { - d_sample_counter = d_sample_counter_next; d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); @@ -2408,8 +2427,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // this must be computed for the secondary prn code if (d_secondary) { - - d_first_length_secondary_code = d_current_integration_length_samples - (trk_parameters.extend_correlation_symbols - 1)*static_cast(std::floor(T_prn_samples)); + d_first_length_secondary_code = d_current_integration_length_samples - (d_fpga_integration_period - 1)*static_cast(std::floor(T_prn_samples)); d_next_length_secondary_code = static_cast(std::floor(T_prn_samples)); multicorrelator_fpga->update_secondary_code_length(d_first_length_secondary_code, d_next_length_secondary_code); @@ -2469,7 +2487,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.correlation_length_ms = d_correlation_length_ms; - d_num_current_syncrho_repetitions = trk_parameters.extend_correlation_symbols; + d_num_current_syncrho_repetitions = d_fpga_integration_period; d_extend_correlation_symbols_count = 0; @@ -2482,6 +2500,10 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_L_accu = gr_complex(0.0, 0.0); d_VL_accu = gr_complex(0.0, 0.0); + if (d_extend_fpga_integration_periods > 1) + { + d_state = 5; + } } @@ -2502,7 +2524,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // debug d_sc_prompt_changed = false; - if (d_state == 3 || d_state == 4 | d_state ==6) + if (d_state == 3 || d_state == 4 || d_state == 5 || d_state ==6) { // debug - remodulate secondary code if (d_sc_remodulate_enabled == true) @@ -2578,7 +2600,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un current_synchro_data.Carrier_phase_rads = d_extended_integration_first_acc_carrier_phase_rad; d_sc_prompt_changed = false; - if (d_state == 3 || d_state == 4 | d_state ==6) + if (d_state == 3 || d_state == 4 || d_state == 5 || d_state ==6) { if (d_sc_remodulate_enabled == true) From fc18eba4dd72dab755ca3a925452a6882f1de1ed Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Tue, 9 Jul 2019 12:17:56 +0200 Subject: [PATCH 06/22] removed tracking state 0 --- .../dll_pll_veml_tracking_fpga.cc | 51 ++++++++++++++----- .../dll_pll_veml_tracking_fpga.h | 5 ++ 2 files changed, 44 insertions(+), 12 deletions(-) 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 66ec7025f..de01987b7 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 @@ -501,7 +501,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_carrier_phase_step_rad = 0.0; d_carrier_phase_rate_step_rad = 0.0; d_rem_code_phase_chips = 0.0; - d_state = 0; // initial state: standby + d_state = 1; // initial state: standby clear_tracking_vars(); if (trk_parameters.smoother_length > 0) @@ -567,6 +567,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & std::ofstream outfile; outfile.open("trk_out.txt", std::ios_base::trunc); outfile.close(); + d_worker_is_done = false; } void dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) @@ -621,8 +622,14 @@ void dll_pll_veml_tracking_fpga::start_tracking() d_carrier_loop_filter.initialize(static_cast(d_acq_carrier_doppler_hz)); // initialize the carrier filter + + boost::mutex::scoped_lock lock(d_mutex); + d_worker_is_done = true; + m_condition.notify_one(); + + // enable tracking pull-in - d_state = 1; + //d_state = 1; } @@ -1814,7 +1821,7 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) void dll_pll_veml_tracking_fpga::stop_tracking() { printf("stop tracking called\n"); - d_state = 0; + d_state = 1; } @@ -1850,14 +1857,34 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un switch (d_state) { - case 0: // Standby - Consume samples at full throttle, do nothing - { - *out[0] = *d_acquisition_gnss_synchro; - usleep(1000); - return 1; - } +// case 0: // Standby - Consume samples at full throttle, do nothing +// { +// +// // EL MUTEX NO FA FALTA !! GNURADIO JA EN TE UN ARREGLAR +// printf("in state 0\n"); +// d_worker_is_done = false; +// +// +// boost::mutex::scoped_lock lock(d_mutex); +// while (!d_worker_is_done) m_condition.wait(lock); +// +// d_state = 1; +// +// //*out[0] = *d_acquisition_gnss_synchro; +// ////usleep(1000); +// //return 1; +// } case 1: // Pull-in { + + // EL MUTEX NO FA FALTA !! GNURADIO JA EN TE UN ARREGLAR + printf("in state 1\n"); + d_worker_is_done = false; + + + boost::mutex::scoped_lock lock(d_mutex); + while (!d_worker_is_done) m_condition.wait(lock); + // Signal alignment (skip samples until the incoming signal is aligned with local replica) int64_t acq_trk_diff_samples; @@ -1952,7 +1979,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un if (!cn0_and_tracking_lock_status(d_code_period)) { clear_tracking_vars(); - d_state = 0; // loss-of-lock detected + d_state = 1; // loss-of-lock detected } else { @@ -2230,7 +2257,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) { clear_tracking_vars(); - d_state = 0; // loss-of-lock detected + d_state = 1; // loss-of-lock detected } else { @@ -2397,7 +2424,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) { clear_tracking_vars(); - d_state = 0; // loss-of-lock detected + d_state = 1; // loss-of-lock detected } else { diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index af7cfd659..5eabb27a9 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -281,6 +281,11 @@ private: uint32_t d_fpga_integration_period; + bool d_worker_is_done; + + boost::condition_variable m_condition; + boost::mutex d_mutex; + }; From 3095176524e811100f4aea1d1aa04cbe9878cbce Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Tue, 9 Jul 2019 19:15:27 +0200 Subject: [PATCH 07/22] added the bit synchronization time limit and the doppler correction --- .../dll_pll_veml_tracking_fpga.cc | 39 +++++++++++++++++++ .../dll_pll_veml_tracking_fpga.h | 3 ++ .../tracking/libs/dll_pll_conf_fpga.cc | 4 +- .../tracking/libs/dll_pll_conf_fpga.h | 2 + 4 files changed, 47 insertions(+), 1 deletion(-) 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 de01987b7..98f1433a8 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 @@ -58,6 +58,7 @@ #include #include #include +#include #include #if HAS_STD_FILESYSTEM @@ -96,6 +97,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & this->set_msg_handler(pmt::mp("telemetry_to_trk"), boost::bind(&dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk, this, _1)); // initialize internal vars + d_dll_filt_history.set_capacity(2000); d_veml = false; d_cloop = true; d_pull_in_transitory = true; @@ -563,11 +565,15 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_num_current_syncrho_repetitions = 1; + d_corrected_doppler = false; + // debug - erase previous outfile contents std::ofstream outfile; outfile.open("trk_out.txt", std::ios_base::trunc); outfile.close(); d_worker_is_done = false; + + } void dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) @@ -623,6 +629,8 @@ void dll_pll_veml_tracking_fpga::start_tracking() d_carrier_loop_filter.initialize(static_cast(d_acq_carrier_doppler_hz)); // initialize the carrier filter + d_corrected_doppler = false; + boost::mutex::scoped_lock lock(d_mutex); d_worker_is_done = true; m_condition.notify_one(); @@ -847,6 +855,29 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() // New code Doppler frequency estimation d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate - d_code_error_filt_chips; + + // Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler + if (trk_parameters.enable_doppler_correction == true) + { + if (d_pull_in_transitory == false and d_corrected_doppler == false) + { + d_dll_filt_history.push_back(static_cast(d_code_error_filt_chips)); + + if (d_dll_filt_history.full()) + { + float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast(d_dll_filt_history.capacity()); + if (fabs(avg_code_error_chips_s) > 1.0) + { + float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(d_code_chip_rate); + LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + d_carrier_loop_filter.initialize(d_carrier_doppler_hz - carrier_doppler_error_hz); + d_corrected_doppler = true; + } + d_dll_filt_history.clear(); + } + } + } + } @@ -1974,6 +2005,14 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_P_accu = *d_Prompt; d_L_accu = *d_Late; + //fail-safe: check if the secondary code or bit synchronization has not succedded 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)) + { + d_carrier_lock_fail_counter = 300000; //force loss-of-lock condition + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + // Check lock status if (!cn0_and_tracking_lock_status(d_code_period)) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 5eabb27a9..850786cdf 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -124,6 +124,8 @@ private: int32_t d_preamble_length_symbols; boost::circular_buffer d_symbol_history; + // dll filter buffer + boost::circular_buffer d_dll_filt_history; // tracking state machine int32_t d_state; @@ -180,6 +182,7 @@ private: // tracking vars bool d_pull_in_transitory; + bool d_corrected_doppler; double d_current_correlation_time_s; double d_carr_phase_error_hz; double d_carr_freq_error_hz; diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc index 88c6ead1d..74be48fda 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc @@ -46,7 +46,8 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() dump_filename = std::string("./dll_pll_dump.dat"); enable_fll_pull_in = false; enable_fll_steady_state = false; - pull_in_time_s = 2; + pull_in_time_s = 10; + bit_synchronization_time_limit_s = pull_in_time_s + 60; fll_filter_order = 1; pll_filter_order = 3; dll_filter_order = 2; @@ -66,6 +67,7 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() cn0_min = 25; max_lock_fail = 50; carrier_lock_th = 0.85; + enable_doppler_correction = false; track_pilot = false; system = 'G'; char sig_[3] = "1C"; diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h index c61066e84..48acbe624 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h @@ -46,6 +46,7 @@ public: bool enable_fll_pull_in; bool enable_fll_steady_state; unsigned int pull_in_time_s; // signed integer, when pull in time is not yet reached it has to be compared against a negative number + unsigned int bit_synchronization_time_limit_s; int pll_filter_order; int dll_filter_order; @@ -74,6 +75,7 @@ public: uint32_t smoother_length; double carrier_lock_th; bool track_pilot; + bool enable_doppler_correction; char system; char signal[3]; std::string device_name; From 718fc034424783f829ca4333522720c7ff860aea Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Tue, 9 Jul 2019 20:11:21 +0200 Subject: [PATCH 08/22] removed integrating parameter from log_data() --- .../dll_pll_veml_tracking_fpga.cc | 44 +++++++++---------- .../dll_pll_veml_tracking_fpga.h | 9 +++- 2 files changed, 29 insertions(+), 24 deletions(-) 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 98f1433a8..f3e74272b 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 @@ -97,7 +97,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & this->set_msg_handler(pmt::mp("telemetry_to_trk"), boost::bind(&dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk, this, _1)); // initialize internal vars - d_dll_filt_history.set_capacity(2000); + d_dll_filt_history.set_capacity(1000); d_veml = false; d_cloop = true; d_pull_in_transitory = true; @@ -1153,7 +1153,7 @@ void dll_pll_veml_tracking_fpga::save_correlation_results_extended_integration_i } } -void dll_pll_veml_tracking_fpga::log_data(bool integrating) +void dll_pll_veml_tracking_fpga::log_data() { if (d_dump) { @@ -1203,20 +1203,20 @@ void dll_pll_veml_tracking_fpga::log_data(bool integrating) tmp_E = std::abs(d_E_accu); tmp_P = std::abs(d_P_accu); tmp_L = std::abs(d_L_accu); - if (integrating) - { - //TODO: Improve this solution! - // It compensates the amplitude difference while integrating - if (d_extend_correlation_symbols_count > 0) - { - float scale_factor = static_cast(trk_parameters.extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); - tmp_VE *= scale_factor; - tmp_E *= scale_factor; - tmp_P *= scale_factor; - tmp_L *= scale_factor; - tmp_VL *= scale_factor; - } - } +// if (integrating) +// { +// //TODO: Improve this solution! +// // It compensates the amplitude difference while integrating +// if (d_extend_correlation_symbols_count > 0) +// { +// float scale_factor = static_cast(trk_parameters.extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); +// tmp_VE *= scale_factor; +// tmp_E *= scale_factor; +// tmp_P *= scale_factor; +// tmp_L *= scale_factor; +// tmp_VL *= scale_factor; +// } +// } try { @@ -1280,7 +1280,7 @@ void dll_pll_veml_tracking_fpga::log_data(bool integrating) } -void dll_pll_veml_tracking_fpga::log_data_extended_integration_in_FPGA(bool integrating, bool extended_correlation_in_fpga_enabled) +void dll_pll_veml_tracking_fpga::log_data_extended_integration_in_FPGA(bool extended_correlation_in_fpga_enabled) { if (d_dump) { @@ -2032,7 +2032,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_num_current_syncrho_repetitions = 1; // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(false); + log_data(); if (d_secondary) { @@ -2278,7 +2278,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_state = 4; } d_num_current_syncrho_repetitions = 1; - log_data(true); + log_data(); break; } case 4: // narrow tracking @@ -2343,7 +2343,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_num_current_syncrho_repetitions = 1; - log_data(false); + log_data(); // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); d_E_accu = gr_complex(0.0, 0.0); @@ -2430,7 +2430,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un } - log_data_extended_integration_in_FPGA(false, true); + log_data_extended_integration_in_FPGA(true); break; } @@ -2513,7 +2513,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_extend_correlation_symbols_count = 0; // enable write dump file this cycle (valid DLL/PLL cycle) - log_data_extended_integration_in_FPGA(false, true); + log_data_extended_integration_in_FPGA(true); // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); d_E_accu = gr_complex(0.0, 0.0); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 850786cdf..907db7dbb 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -92,8 +92,8 @@ private: void clear_tracking_vars(); void save_correlation_results(); void save_correlation_results_extended_integration_in_FPGA(); - void log_data(bool integrating); - void log_data_extended_integration_in_FPGA(bool integrating, bool extended_correlation_in_fpga_enabled); + void log_data(); + void log_data_extended_integration_in_FPGA(bool extended_correlation_in_fpga_enabled); int32_t save_matfile(); //void run_state_2(Gnss_Synchro ¤t_synchro_data); @@ -112,12 +112,14 @@ private: double d_code_period; double d_code_chip_rate; uint32_t d_secondary_code_length; + uint32_t d_data_secondary_code_length; uint32_t d_code_length_chips; uint32_t d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled) int32_t d_symbols_per_bit; std::string systemName; std::string signal_type; std::string *d_secondary_code_string; + std::string *d_data_secondary_code_string; std::string signal_pretty_name; int32_t *d_preambles_symbols; @@ -146,6 +148,8 @@ private: bool d_enable_extended_integration; int32_t d_extend_correlation_symbols_count; int32_t d_current_symbol; + int32_t d_current_data_symbol; + gr_complex d_VE_accu; gr_complex d_E_accu; @@ -157,6 +161,7 @@ private: // gr_complex d_P_data_accu; // when the extended integration is done in the FPGA we need to accumulate the pilot correlator results too uint32_t d_num_current_syncrho_repetitions; + gr_complex d_P_data_accu; gr_complex *d_Prompt_Data; double d_code_phase_step_chips; From ca9e8cd9b58799a7905d30a486df52ca2190abd6 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Wed, 10 Jul 2019 18:02:57 +0200 Subject: [PATCH 09/22] updated the tracking module. Added some variables to adapt it to the new telemetry modules. --- .../dll_pll_veml_tracking_fpga.cc | 297 +++++++++++++----- 1 file changed, 217 insertions(+), 80 deletions(-) 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 f3e74272b..f0b67b789 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 @@ -76,6 +76,9 @@ namespace fs = boost::filesystem; #define MAX_INTEGR_TIME_PRN_SYMB 20 +const std::string GPS_CA_PREAMBLE_SYMBOLS_STR = {"1111111111111111111100000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000001111111111111111111111111111111111111111"}; + + dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_) { return dll_pll_veml_tracking_fpga_sptr(new dll_pll_veml_tracking_fpga(conf_)); @@ -104,6 +107,9 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_code_chip_rate = 0.0; d_secondary_code_length = 0U; d_secondary_code_string = nullptr; + d_data_secondary_code_length = 0U; + d_data_secondary_code_string = nullptr; + // used by the FPGA d_secondary_code_length_data = 0U; @@ -182,6 +188,13 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_symbol_history.set_capacity(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size d_symbol_history.clear(); // Clear all the elements in the buffer + // 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); + d_secondary_code_string = const_cast(&GPS_CA_PREAMBLE_SYMBOLS_STR); + + + } else if (signal_type == "2S") { @@ -200,6 +213,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_signal_carrier_freq = GPS_L5_FREQ_HZ; d_code_period = GPS_L5I_PERIOD; d_code_chip_rate = GPS_L5I_CODE_RATE_HZ; + // symbol integration: 10 trk symbols (10 ms) = 1 tlm bit d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 1; d_secondary = true; @@ -217,8 +231,13 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & if (trk_parameters.track_pilot) { + // synchronize pilot secondary code d_secondary_code_length = static_cast(GPS_L5Q_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5Q_NH_CODE_STR); + // remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) + d_data_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "Q"; interchange_iq = true; @@ -235,7 +254,8 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & } else { - + // synchronize and remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) d_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "I"; @@ -279,6 +299,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_signal_carrier_freq = GALILEO_E1_FREQ_HZ; d_code_period = GALILEO_E1_CODE_PERIOD; d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_HZ; + // Galileo E1b has 1 trk symbol (4 ms) per tlm bit, no symbol integration required d_symbols_per_bit = 1; d_correlation_length_ms = 4; d_veml = true; @@ -294,7 +315,9 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_secondary = false; signal_pretty_name = signal_pretty_name + "B"; } - interchange_iq = false; // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. + interchange_iq = false; + // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. + } else if (signal_type == "5X") { @@ -307,22 +330,37 @@ 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; + + if (d_extended_correlation_in_fpga == true) + { + if (trk_parameters.extend_correlation_symbols > 1) + { + d_sc_demodulate_enabled = true; + d_sc_remodulate_enabled = true; + } + } + if (trk_parameters.track_pilot) { - if (d_extended_correlation_in_fpga == true) - { - if (trk_parameters.extend_correlation_symbols > 1) - { - d_sc_demodulate_enabled = true; - d_sc_remodulate_enabled = true; - } - } +// if (d_extended_correlation_in_fpga == true) +// { +// if (trk_parameters.extend_correlation_symbols > 1) +// { +// d_sc_demodulate_enabled = true; +// d_sc_remodulate_enabled = true; +// } +// } - d_secondary = true; +// d_secondary = true; d_secondary_code_length = static_cast(GALILEO_E5A_Q_SECONDARY_CODE_LENGTH); - signal_pretty_name = signal_pretty_name + "Q"; + // remove data secondary code + d_data_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); + + interchange_iq = true; // used by the FPGA @@ -343,7 +381,15 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & //Do not acquire secondary code in data component. It is done in telemetry decoder - d_secondary = false; +// d_secondary = false; + + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); + + d_secondary_code_string_post_apply = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); + d_secondary_code_post_apply_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + signal_pretty_name = signal_pretty_name + "I"; interchange_iq = false; @@ -591,7 +637,7 @@ void dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk(const pmt::pmt_t & { DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; gr::thread::scoped_lock lock(d_setlock); - d_carrier_lock_fail_counter = 10000; //force loss-of-lock condition + d_carrier_lock_fail_counter = 200000; //force loss-of-lock condition break; } default: @@ -887,6 +933,7 @@ void dll_pll_veml_tracking_fpga::clear_tracking_vars() if (trk_parameters.track_pilot) { d_Prompt_Data[0] = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); } d_P_accu_old = gr_complex(0.0, 0.0); d_carr_phase_error_hz = 0.0; @@ -895,6 +942,7 @@ void dll_pll_veml_tracking_fpga::clear_tracking_vars() d_code_error_chips = 0.0; d_code_error_filt_chips = 0.0; d_current_symbol = 0; + d_current_data_symbol = 0; d_Prompt_circular_buffer.clear(); d_carrier_phase_rate_step_rad = 0.0; d_code_phase_rate_step_chips = 0.0; @@ -1115,9 +1163,71 @@ void dll_pll_veml_tracking_fpga::save_correlation_results() d_E_accu += *d_Early; d_P_accu += *d_Prompt; d_L_accu += *d_Late; + d_current_symbol++; d_current_symbol %= d_symbols_per_bit; } + + // data secondary code roll-up + if (d_symbols_per_bit > 1) + { + if (d_data_secondary_code_length > 0) + { + if (trk_parameters.track_pilot) + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt_Data; + } + else + { + d_P_data_accu -= *d_Prompt_Data; + } + } + else + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt; + } + else + { + d_P_data_accu -= *d_Prompt; + } + } + + d_current_data_symbol++; + // data secondary code roll-up + d_current_data_symbol %= d_data_secondary_code_length; + } + else + { + if (trk_parameters.track_pilot) + { + d_P_data_accu += *d_Prompt_Data; + } + else + { + d_P_data_accu += *d_Prompt; + //std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl; + } + d_current_data_symbol++; + d_current_data_symbol %= d_symbols_per_bit; + } + } + else + { + if (trk_parameters.track_pilot) + { + d_P_data_accu = *d_Prompt_Data; + } + else + { + d_P_data_accu = *d_Prompt; + } + } + + // If tracking pilot, disable Costas loop if (trk_parameters.track_pilot) { @@ -1870,6 +1980,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un { auto **out = reinterpret_cast(&output_items[0]); Gnss_Synchro current_synchro_data = Gnss_Synchro(); + current_synchro_data.Flag_valid_symbol_output = false; d_current_integration_length_samples = d_next_integration_length_samples; @@ -1989,8 +2100,8 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un { d_sample_counter = d_sample_counter_next; d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - uint64_t d_sample_counter_tmp = d_sample_counter; - uint64_t d_sample_counter_next_tmp = d_sample_counter_next; + //uint64_t d_sample_counter_tmp = d_sample_counter; + //uint64_t d_sample_counter_next_tmp = d_sample_counter_next; do_correlation_step(); @@ -2034,72 +2145,94 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // enable write dump file this cycle (valid DLL/PLL cycle) log_data(); - if (d_secondary) + if (!d_pull_in_transitory) { - // ####### SECONDARY CODE LOCK ##### - d_Prompt_circular_buffer.push_back(*d_Prompt); + if (d_secondary) + { + // ####### SECONDARY CODE LOCK ##### + d_Prompt_circular_buffer.push_back(*d_Prompt); - if (d_Prompt_circular_buffer.size() == d_secondary_code_length) - { - next_state = acquire_secondary(); + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); - if (next_state) - { - LOG(INFO) << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - } - } - } - else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change - { - float current_tracking_time_s = static_cast(d_sample_counter - d_absolute_samples_offset) / trk_parameters.fs_in; - - if (current_tracking_time_s > 10) - { - d_symbol_history.push_back(d_Prompt->real()); - - //******* preamble correlation ******** - - int32_t corr_value = 0; - if ((static_cast(d_symbol_history.size()) == d_preamble_length_symbols)) // and (d_make_correlation or !d_flag_frame_sync)) - { - int i = 0; - for (const auto &iter : d_symbol_history) - { - if (iter < 0.0) // symbols clipping - { - corr_value -= d_preambles_symbols[i]; - } - else - { - corr_value += d_preambles_symbols[i]; - } - i++; - } - } - - if (corr_value == d_preamble_length_symbols) - { - LOG(INFO) << systemName << " " << signal_pretty_name << " tracking preamble detected in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - next_state = true; - } - else - { - next_state = false; - } - } - else - { - next_state = false; - } - } - else - { - next_state = true; + if (next_state) + { + LOG(INFO) << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + } + } + else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change + { +// float current_tracking_time_s = static_cast(d_sample_counter - d_absolute_samples_offset) / trk_parameters.fs_in; +// +// if (current_tracking_time_s > 10) +// { +// d_symbol_history.push_back(d_Prompt->real()); +// +// //******* preamble correlation ******** +// +// int32_t corr_value = 0; +// if ((static_cast(d_symbol_history.size()) == d_preamble_length_symbols)) // and (d_make_correlation or !d_flag_frame_sync)) +// { +// int i = 0; +// for (const auto &iter : d_symbol_history) +// { +// if (iter < 0.0) // symbols clipping +// { +// corr_value -= d_preambles_symbols[i]; +// } +// else +// { +// corr_value += d_preambles_symbols[i]; +// } +// i++; +// } +// } +// +// if (corr_value == d_preamble_length_symbols) +// { +// LOG(INFO) << systemName << " " << signal_pretty_name << " tracking preamble detected in channel " << d_channel +// << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; +// next_state = true; +// } +// else +// { +// next_state = false; +// } +// } +// else +// { +// next_state = false; +// } + + //******* preamble correlation ******** + d_Prompt_circular_buffer.push_back(*d_Prompt); + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); + if (next_state) + { + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + std::cout << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + } + + } + else + { + next_state = true; + } } + else + { + next_state = false; //keep in state 2 during pull-in transitory + } // ########### Output the tracking results to Telemetry block ########## if (interchange_iq) @@ -2144,10 +2277,12 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un d_VE_accu = gr_complex(0.0, 0.0); d_E_accu = gr_complex(0.0, 0.0); d_P_accu = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); d_L_accu = gr_complex(0.0, 0.0); d_VL_accu = gr_complex(0.0, 0.0); d_Prompt_circular_buffer.clear(); d_current_symbol = 0; + d_current_data_symbol = 0; if (d_enable_extended_integration) { @@ -2232,8 +2367,9 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // perform a correlation step do_correlation_step(); - update_tracking_vars(); + //update_tracking_vars(); save_correlation_results(); + update_tracking_vars(); // ########### Output the tracking results to Telemetry block ########## if (interchange_iq) @@ -2380,8 +2516,9 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // perform a correlation step do_correlation_step(); - update_tracking_vars_extend_integration_in_FPGA(); + //update_tracking_vars_extend_integration_in_FPGA(); save_correlation_results_extended_integration_in_FPGA(); + update_tracking_vars_extend_integration_in_FPGA(); // ########### Output the tracking results to Telemetry block ########## if (interchange_iq) From 4c22a1ce1a65558ceefdd44d8310824a79216cc1 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Wed, 17 Jul 2019 18:01:27 +0200 Subject: [PATCH 10/22] the tracking module is now sending 1 synchro_data per data symbol. --- ...lileo_e1_pcps_ambiguous_acquisition_fpga.h | 15 +- .../galileo_e5a_pcps_acquisition_fpga.h | 13 +- .../gps_l1_ca_pcps_acquisition_fpga.h | 9 +- .../adapters/gps_l5i_pcps_acquisition_fpga.h | 10 +- .../galileo_e1_dll_pll_veml_tracking_fpga.cc | 43 +- .../galileo_e1_dll_pll_veml_tracking_fpga.h | 13 +- .../galileo_e5a_dll_pll_tracking_fpga.cc | 45 +- .../galileo_e5a_dll_pll_tracking_fpga.h | 9 +- .../gps_l1_ca_dll_pll_tracking_fpga.cc | 45 +- .../gps_l1_ca_dll_pll_tracking_fpga.h | 9 +- .../gps_l2_m_dll_pll_tracking_fpga.cc | 25 +- .../adapters/gps_l5_dll_pll_tracking_fpga.cc | 47 +- .../adapters/gps_l5_dll_pll_tracking_fpga.h | 10 +- .../dll_pll_veml_tracking_fpga.cc | 2016 +++++------------ .../dll_pll_veml_tracking_fpga.h | 99 +- .../tracking/libs/dll_pll_conf_fpga.cc | 11 +- .../tracking/libs/dll_pll_conf_fpga.h | 6 +- 17 files changed, 738 insertions(+), 1687 deletions(-) diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h index a92b65e31..0b2da5998 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h @@ -24,7 +24,7 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -32,16 +32,15 @@ #ifndef GNSS_SDR_GALILEO_E1_PCPS_AMBIGUOUS_ACQUISITION_FPGA_H_ #define GNSS_SDR_GALILEO_E1_PCPS_AMBIGUOUS_ACQUISITION_FPGA_H_ +#include "acq_conf.h" #include "channel_fsm.h" +#include "gnss_synchro.h" #include "pcps_acquisition_fpga.h" -#include // for basic_block_sptr, top_block_sptr -#include // for lv_16sc_t -#include // for size_t #include #include #include -class Gnss_Synchro; + class ConfigurationInterface; /*! @@ -73,8 +72,7 @@ public: size_t item_size() override { - size_t item_size = sizeof(lv_16sc_t); - return item_size; + return item_size_; } void connect(gr::top_block_sptr top_block) override; @@ -85,7 +83,7 @@ public: /*! * \brief Set acquisition/tracking common Gnss_Synchro object pointer * to efficiently exchange synchronization data between acquisition and - * tracking blocks + * tracking blocks */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; @@ -157,6 +155,7 @@ public: private: ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; + size_t item_size_; bool acquire_pilot_; uint32_t channel_; std::weak_ptr channel_fsm_; diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h index 37e0bf9dd..7e0c75b7d 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h @@ -24,7 +24,7 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -34,16 +34,12 @@ #include "channel_fsm.h" +#include "gnss_synchro.h" #include "pcps_acquisition_fpga.h" -#include // for basic_block_sptr, top_block_sptr -#include // for lv_16sc_t -#include // for size_t -#include #include #include #include -class Gnss_Synchro; class ConfigurationInterface; @@ -76,7 +72,7 @@ public: inline size_t item_size() override { - return sizeof(lv_16sc_t); + return item_size_; } void connect(gr::top_block_sptr top_block) override; @@ -87,7 +83,7 @@ public: /*! * \brief Set acquisition/tracking common Gnss_Synchro object pointer * to efficiently exchange synchronization data between acquisition and - * tracking blocks + * tracking blocks */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; @@ -169,6 +165,7 @@ public: private: ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; + size_t item_size_; std::string item_type_; std::string dump_filename_; std::string role_; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h index 39b8d2e1f..6c20bb481 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h @@ -38,14 +38,11 @@ #include "channel_fsm.h" #include "pcps_acquisition_fpga.h" -#include // for basic_block_sptr, top_block_sptr -#include // for lv_16sc_t -#include // for size_t #include #include #include -class Gnss_Synchro; + class ConfigurationInterface; /*! @@ -77,8 +74,7 @@ public: inline size_t item_size() override { - size_t item_size = sizeof(lv_16sc_t); - return item_size; + return item_size_; } void connect(gr::top_block_sptr top_block) override; @@ -161,6 +157,7 @@ public: private: ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; + size_t item_size_; uint32_t channel_; std::weak_ptr channel_fsm_; uint32_t doppler_max_; diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h index 1d5c8e922..d3607002e 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h @@ -27,7 +27,7 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -37,14 +37,11 @@ #include "channel_fsm.h" #include "pcps_acquisition_fpga.h" -#include // for basic_block_sptr, top_block_sptr -#include // for lv_16sc_t -#include // for size_t #include #include #include -class Gnss_Synchro; + class ConfigurationInterface; /*! @@ -76,7 +73,7 @@ public: inline size_t item_size() override { - return sizeof(lv_16sc_t); + return item_size_; } void connect(gr::top_block_sptr top_block) override; @@ -159,6 +156,7 @@ public: private: ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; + size_t item_size_; std::string item_type_; uint32_t channel_; std::weak_ptr channel_fsm_; diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc index 03e6afe6d..719798438 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc @@ -29,7 +29,7 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -41,12 +41,8 @@ #include "dll_pll_conf_fpga.h" #include "galileo_e1_signal_processing.h" #include "gnss_sdr_flags.h" -#include "gnss_synchro.h" #include #include -#include // for round -#include // for memcpy -#include // for operator<<, // the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. @@ -81,7 +77,6 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( { trk_param_fpga.smoother_length = configuration->property(role + ".smoother_length", 10); } - float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0); if (FLAGS_pll_bw_hz != 0.0) { @@ -173,31 +168,19 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( trk_param_fpga.system = 'E'; char sig_[3] = "1B"; std::memcpy(trk_param_fpga.signal, sig_, 3); - int32_t cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } + trk_param_fpga.cn0_samples = configuration->property(role + ".cn0_samples", trk_param_fpga.cn0_samples); + trk_param_fpga.cn0_min = configuration->property(role + ".cn0_min", trk_param_fpga.cn0_min); + trk_param_fpga.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param_fpga.max_code_lock_fail); + trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); + trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); + +// int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); +// if (FLAGS_max_lock_fail != 50) +// { +// max_lock_fail = FLAGS_max_lock_fail; +// } +// trk_param_fpga.max_lock_fail = max_lock_fail; - trk_param_fpga.cn0_samples = cn0_samples; - int32_t cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param_fpga.cn0_min = cn0_min; - int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param_fpga.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param_fpga.carrier_lock_th = carrier_lock_th; // FPGA configuration parameters std::string default_device_name = "/dev/uio"; diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h index 690f0039f..a81cd7d02 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h @@ -29,7 +29,7 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -39,12 +39,9 @@ #include "dll_pll_veml_tracking_fpga.h" #include "tracking_interface.h" -#include // for basic_block_sptr, basic_block_sptr -#include // for size_t -#include // for uint32_t -#include // for string +#include + -class Gnss_Synchro; class ConfigurationInterface; /*! @@ -74,7 +71,7 @@ public: inline size_t item_size() override { - return sizeof(int); + return item_size_; } void connect(gr::top_block_sptr top_block) override; @@ -95,7 +92,6 @@ public: void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; void start_tracking() override; - /*! * \brief Stop running tracking */ @@ -103,6 +99,7 @@ public: private: dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; + size_t item_size_; uint32_t channel_; std::string role_; uint32_t in_streams_; diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index a9dda0968..f0e5a13ce 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -24,7 +24,7 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -36,14 +36,8 @@ #include "dll_pll_conf_fpga.h" #include "galileo_e5_signal_processing.h" #include "gnss_sdr_flags.h" -#include "gnss_synchro.h" #include -#include // for gr_complex #include -#include // for round -#include -#include // for memcpy -#include // the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. @@ -167,30 +161,19 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( trk_param_fpga.system = 'E'; char sig_[3] = "5X"; std::memcpy(trk_param_fpga.signal, sig_, 3); - int32_t cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - trk_param_fpga.cn0_samples = cn0_samples; - int32_t cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param_fpga.cn0_min = cn0_min; - int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param_fpga.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param_fpga.carrier_lock_th = carrier_lock_th; + trk_param_fpga.cn0_samples = configuration->property(role + ".cn0_samples", trk_param_fpga.cn0_samples); + trk_param_fpga.cn0_min = configuration->property(role + ".cn0_min", trk_param_fpga.cn0_min); + trk_param_fpga.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param_fpga.max_code_lock_fail); + trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); + trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); + +// int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); +// if (FLAGS_max_lock_fail != 50) +// { +// max_lock_fail = FLAGS_max_lock_fail; +// } +// trk_param_fpga.max_lock_fail = max_lock_fail; + d_data_codes = nullptr; diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h index 1950e4630..48a029a22 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h @@ -24,7 +24,7 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -34,12 +34,8 @@ #include "dll_pll_veml_tracking_fpga.h" #include "tracking_interface.h" -#include // for basic_block_sptr -#include // For uint32_t -#include // for size_t #include -class Gnss_Synchro; class ConfigurationInterface; /*! @@ -68,7 +64,7 @@ public: inline size_t item_size() override { - return sizeof(int); + return item_size_; } void connect(gr::top_block_sptr top_block) override; @@ -95,6 +91,7 @@ public: private: dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; + size_t item_size_; uint32_t channel_; std::string role_; uint32_t in_streams_; diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index aecebe73f..2f0e7fb97 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -29,7 +29,7 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -40,13 +40,9 @@ #include "display.h" #include "dll_pll_conf_fpga.h" #include "gnss_sdr_flags.h" -#include "gnss_synchro.h" #include "gps_sdr_signal_processing.h" #include #include -#include // for round -#include // for memcpy -#include #define NUM_PRNs 32 // total number of PRNs #define GPS_CA_BIT_DURATION_MS 20 @@ -61,7 +57,6 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( { Dll_Pll_Conf_Fpga trk_param_fpga = Dll_Pll_Conf_Fpga(); DLOG(INFO) << "role " << role; - //################# CONFIGURATION PARAMETERS ######################## int32_t fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); int32_t fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); @@ -76,7 +71,6 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( { trk_param_fpga.smoother_length = configuration->property(role + ".smoother_length", 10); } - bool dump = configuration->property(role + ".dump", false); trk_param_fpga.dump = dump; std::string default_dump_filename = "./track_ch"; @@ -176,30 +170,19 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( trk_param_fpga.system = 'G'; char sig_[3] = "1C"; std::memcpy(trk_param_fpga.signal, sig_, 3); - int32_t cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - trk_param_fpga.cn0_samples = cn0_samples; - int32_t cn0_min = configuration->property(role + ".cn0_min", 30); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param_fpga.cn0_min = cn0_min; - int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param_fpga.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.80); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param_fpga.carrier_lock_th = carrier_lock_th; + trk_param_fpga.cn0_samples = configuration->property(role + ".cn0_samples", trk_param_fpga.cn0_samples); + trk_param_fpga.cn0_min = configuration->property(role + ".cn0_min", trk_param_fpga.cn0_min); + trk_param_fpga.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param_fpga.max_code_lock_fail); + trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); + trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); + + //int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); + //if (FLAGS_max_lock_fail != 50) + // { + // max_lock_fail = FLAGS_max_lock_fail; + // } + //trk_param_fpga.max_lock_fail = max_lock_fail; + // FPGA configuration parameters std::string default_device_name = "/dev/uio"; diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h index 680c903f2..ce3ce90fd 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h @@ -29,7 +29,7 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -39,12 +39,8 @@ #include "dll_pll_veml_tracking_fpga.h" #include "tracking_interface.h" -#include -#include #include - -class Gnss_Synchro; class ConfigurationInterface; /*! @@ -73,7 +69,7 @@ public: inline size_t item_size() override { - return sizeof(int); + return item_size_; } void connect(gr::top_block_sptr top_block) override; @@ -101,6 +97,7 @@ public: private: dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; + size_t item_size_; uint32_t channel_; std::string role_; uint32_t in_streams_; diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc index 6cf1c2071..a17a68e3f 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc @@ -100,18 +100,19 @@ GpsL2MDllPllTrackingFpga::GpsL2MDllPllTrackingFpga( trk_param_fpga.system = 'G'; char sig_[3] = "2S"; std::memcpy(trk_param_fpga.signal, sig_, 3); - int cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; - trk_param_fpga.cn0_samples = cn0_samples; - int cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) cn0_min = FLAGS_cn0_min; - trk_param_fpga.cn0_min = cn0_min; - int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) max_lock_fail = FLAGS_max_lock_fail; - trk_param_fpga.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); - if (FLAGS_carrier_lock_th != 0.85) carrier_lock_th = FLAGS_carrier_lock_th; - trk_param_fpga.carrier_lock_th = carrier_lock_th; + trk_param_fpga.cn0_samples = configuration->property(role + ".cn0_samples", trk_param_fpga.cn0_samples); + trk_param_fpga.cn0_min = configuration->property(role + ".cn0_min", trk_param_fpga.cn0_min); + trk_param_fpga.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param_fpga.max_code_lock_fail); + trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); + trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); + +// int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); +// if (FLAGS_max_lock_fail != 50) +// { +// max_lock_fail = FLAGS_max_lock_fail; +// } +// trk_param_fpga.max_lock_fail = max_lock_fail; + // FPGA configuration parameters std::string default_device_name = "/dev/uio"; diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index b6bc09920..9f05affa7 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -30,7 +30,7 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -42,13 +42,13 @@ #include "display.h" #include "dll_pll_conf_fpga.h" #include "gnss_sdr_flags.h" -#include "gnss_synchro.h" +//#include "gnss_synchro.h" #include "gps_l5_signal.h" #include #include -#include // for round -#include // for memcpy -#include +//#include // for round +//#include // for memcpy +//#include #define NUM_PRNs 32 // number of PRNS @@ -174,30 +174,19 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( trk_param_fpga.system = 'G'; char sig_[3] = "L5"; std::memcpy(trk_param_fpga.signal, sig_, 3); - int32_t cn0_samples = configuration->property(role + ".cn0_samples", 20); - if (FLAGS_cn0_samples != 20) - { - cn0_samples = FLAGS_cn0_samples; - } - trk_param_fpga.cn0_samples = cn0_samples; - int32_t cn0_min = configuration->property(role + ".cn0_min", 25); - if (FLAGS_cn0_min != 25) - { - cn0_min = FLAGS_cn0_min; - } - trk_param_fpga.cn0_min = cn0_min; - int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - if (FLAGS_max_lock_fail != 50) - { - max_lock_fail = FLAGS_max_lock_fail; - } - trk_param_fpga.max_lock_fail = max_lock_fail; - double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.75); - if (FLAGS_carrier_lock_th != 0.85) - { - carrier_lock_th = FLAGS_carrier_lock_th; - } - trk_param_fpga.carrier_lock_th = carrier_lock_th; + trk_param_fpga.cn0_samples = configuration->property(role + ".cn0_samples", trk_param_fpga.cn0_samples); + trk_param_fpga.cn0_min = configuration->property(role + ".cn0_min", trk_param_fpga.cn0_min); + trk_param_fpga.max_code_lock_fail = configuration->property(role + ".max_lock_fail", trk_param_fpga.max_code_lock_fail); + trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); + trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); + + //int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); + //if (FLAGS_max_lock_fail != 50) + // { + // max_lock_fail = FLAGS_max_lock_fail; + // } + //trk_param_fpga.max_lock_fail = max_lock_fail; + // FPGA configuration parameters std::string default_device_name = "/dev/uio"; diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h index 149fa107a..2805dbcef 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h @@ -30,7 +30,7 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -40,12 +40,8 @@ #include "dll_pll_veml_tracking_fpga.h" #include "tracking_interface.h" -#include -#include -#include #include -class Gnss_Synchro; class ConfigurationInterface; /*! @@ -74,7 +70,7 @@ public: inline size_t item_size() override { - return sizeof(int); + return item_size_; } void connect(gr::top_block_sptr top_block) override; @@ -94,7 +90,6 @@ public: void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; void start_tracking() override; - /*! * \brief Stop running tracking */ @@ -102,6 +97,7 @@ public: private: dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; + size_t item_size_; uint32_t channel_; std::string role_; uint32_t in_streams_; 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 f0b67b789..99596172e 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 @@ -41,22 +41,22 @@ #include "Galileo_E5a.h" #include "MATH_CONSTANTS.h" #include "fpga_multicorrelator.h" -#include "gnss_satellite.h" #include "gnss_sdr_create_directory.h" #include "gnss_synchro.h" +#include "gps_sdr_signal_processing.h" #include "lock_detectors.h" #include "tracking_discriminators.h" #include -#include -#include -#include // for mp +#include // for io_signature +#include // for scoped_lock +#include // for Mat_VarCreate +#include // for mp #include -#include -#include -#include -#include // for abs, size_t -#include -#include +#include // for fill_n +#include // for fmod, round, floor +#include // for exception +#include +#include // for cout, cerr #include #include #include @@ -74,11 +74,6 @@ namespace fs = std::filesystem; namespace fs = boost::filesystem; #endif -#define MAX_INTEGR_TIME_PRN_SYMB 20 - -const std::string GPS_CA_PREAMBLE_SYMBOLS_STR = {"1111111111111111111100000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000001111111111111111111111111111111111111111"}; - - dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_) { return dll_pll_veml_tracking_fpga_sptr(new dll_pll_veml_tracking_fpga(conf_)); @@ -86,15 +81,15 @@ dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Co dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_) : gr::block("dll_pll_veml_tracking_fpga", gr::io_signature::make(0, 0, sizeof(lv_16sc_t)), - gr::io_signature::make(1, MAX_INTEGR_TIME_PRN_SYMB, sizeof(Gnss_Synchro))) + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + //prevent telemetry symbols accumulation in output buffers + this->set_max_noutput_items(1); trk_parameters = conf_; // Telemetry bit synchronization message port input this->message_port_register_out(pmt::mp("events")); this->set_relative_rate(1.0 / static_cast(trk_parameters.vector_length)); - // Telemetry bit synchronization message port input (mainly for GPS L1 CA) - this->message_port_register_in(pmt::mp("preamble_samplestamp")); // Telemetry message port input this->message_port_register_in(pmt::mp("telemetry_to_trk")); this->set_msg_handler(pmt::mp("telemetry_to_trk"), boost::bind(&dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk, this, _1)); @@ -109,16 +104,6 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_secondary_code_string = nullptr; d_data_secondary_code_length = 0U; d_data_secondary_code_string = nullptr; - - - // used by the FPGA - d_secondary_code_length_data = 0U; - d_secondary_code_string_data = 0U; //nullptr; - d_secondary_code_length_pilot = 0U; - d_secondary_code_string_pilot = 0U; //nullptr; - - d_preambles_symbols = nullptr; - d_preamble_length_symbols = 0; signal_type = std::string(trk_parameters.signal); std::map map_signal_pretty_name; @@ -134,20 +119,13 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_code_samples_per_chip = trk_parameters.code_samples_per_chip; // number of samples per chip d_code_length_chips = trk_parameters.code_length_chips; - d_extended_correlation_in_fpga = trk_parameters.extended_correlation_in_fpga; - + d_current_extended_correlation_in_fpga = false; d_extend_fpga_integration_periods = trk_parameters.extend_fpga_integration_periods; // by default - d_fpga_integration_period = trk_parameters.fpga_integration_period; // by default - - //printf("d_extended_correlation_in_fpga = %d\n", d_extended_correlation_in_fpga); - d_sc_remodulate_enabled = false; // by default - + d_current_fpga_integration_period = 1; d_sc_demodulate_enabled = false; - d_flag_printout = false; - if (trk_parameters.system == 'G') { systemName = "GPS"; @@ -156,57 +134,27 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_signal_carrier_freq = GPS_L1_FREQ_HZ; d_code_period = GPS_L1_CA_CODE_PERIOD; d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ; - d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; d_correlation_length_ms = 1; // GPS L1 C/A does not have pilot component nor secondary code d_secondary = false; trk_parameters.track_pilot = false; - interchange_iq = false; - - // set the preamble - uint16_t preambles_bits[GPS_CA_PREAMBLE_LENGTH_BITS] = GPS_PREAMBLE; - - // preamble bits to sampled symbols - d_preamble_length_symbols = GPS_CA_PREAMBLE_LENGTH_SYMBOLS; - d_preambles_symbols = static_cast(volk_gnsssdr_malloc(GPS_CA_PREAMBLE_LENGTH_SYMBOLS * sizeof(int32_t), volk_gnsssdr_get_alignment())); - int32_t n = 0; - for (uint16_t preambles_bit : preambles_bits) - { - for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) - { - if (preambles_bit == 1) - { - d_preambles_symbols[n] = 1; - } - else - { - d_preambles_symbols[n] = -1; - } - n++; - } - } - d_symbol_history.set_capacity(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size - d_symbol_history.clear(); // Clear all the elements in the buffer - // 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); d_secondary_code_string = const_cast(&GPS_CA_PREAMBLE_SYMBOLS_STR); - - - + d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; } else if (signal_type == "2S") { d_signal_carrier_freq = GPS_L2_FREQ_HZ; d_code_period = GPS_L2_M_PERIOD; d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; + // 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; // GPS L2 does not have pilot component nor secondary code d_secondary = false; trk_parameters.track_pilot = false; - interchange_iq = false; } else if (signal_type == "L5") { @@ -217,21 +165,17 @@ 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; - if (d_extended_correlation_in_fpga == true) { if (trk_parameters.extend_correlation_symbols > 1) { - //printf("EXTENDED CORRELATION IN FPGA ENABLED => ENABLING SECONDARY CODE REMODULATION\n"); d_sc_demodulate_enabled = true; - d_sc_remodulate_enabled = true; } } - if (trk_parameters.track_pilot) { - // synchronize pilot secondary code + // synchronize pilot secondary code d_secondary_code_length = static_cast(GPS_L5Q_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5Q_NH_CODE_STR); // remove data secondary code @@ -239,43 +183,15 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_data_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); d_data_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "Q"; - interchange_iq = true; - - // used by the FPGA - d_secondary_code_length_data = static_cast(GPS_L5I_NH_CODE_LENGTH); - d_secondary_code_string_data = const_cast(&GPS_L5I_NH_CODE_STR); - - d_secondary_code_length_pilot = static_cast(GPS_L5Q_NH_CODE_LENGTH); - d_secondary_code_string_pilot = const_cast(&GPS_L5Q_NH_CODE_STR); - - d_secondary_code_string_post_apply = const_cast(&GPS_L5I_NH_CODE_STR); - d_secondary_code_post_apply_length = static_cast(GPS_L5I_NH_CODE_LENGTH); - } else { - // synchronize and remove data secondary code - // remove Neuman-Hofman Code (see IS-GPS-705D) + // synchronize and remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) d_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "I"; - interchange_iq = false; - - // used by the FPGA - d_secondary_code_length_data = static_cast(GPS_L5I_NH_CODE_LENGTH); - d_secondary_code_string_data = const_cast(&GPS_L5I_NH_CODE_STR); - - // debug - //printf("warning : secondary code going to the FPGA is bit flipped for debugging\n"); - //d_secondary_code_string_data = const_cast(&GPS_L5I_NH_CODE_STR_TEST); - - - d_secondary_code_string_post_apply = const_cast(&GPS_L5I_NH_CODE_STR); - d_secondary_code_post_apply_length = static_cast(GPS_L5I_NH_CODE_LENGTH); - } - - } else { @@ -283,7 +199,6 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & std::cerr << "Invalid Signal argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_symbols_per_bit = 0; @@ -291,9 +206,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & } else if (trk_parameters.system == 'E') { - - - systemName = "Galileo"; + systemName = "Galileo"; if (signal_type == "1B") { d_signal_carrier_freq = GALILEO_E1_FREQ_HZ; @@ -315,91 +228,42 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_secondary = false; signal_pretty_name = signal_pretty_name + "B"; } - interchange_iq = false; // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. - } else if (signal_type == "5X") { - - - d_signal_carrier_freq = GALILEO_E5A_FREQ_HZ; d_code_period = GALILEO_E5A_CODE_PERIOD; d_code_chip_rate = GALILEO_E5A_CODE_CHIP_RATE_HZ; d_symbols_per_bit = 20; d_correlation_length_ms = 1; - d_secondary = true; - if (d_extended_correlation_in_fpga == true) { if (trk_parameters.extend_correlation_symbols > 1) { d_sc_demodulate_enabled = true; - d_sc_remodulate_enabled = true; } } - if (trk_parameters.track_pilot) { - -// if (d_extended_correlation_in_fpga == true) -// { -// if (trk_parameters.extend_correlation_symbols > 1) -// { -// d_sc_demodulate_enabled = true; -// d_sc_remodulate_enabled = true; -// } -// } - -// d_secondary = true; + // synchronize pilot secondary code d_secondary_code_length = static_cast(GALILEO_E5A_Q_SECONDARY_CODE_LENGTH); signal_pretty_name = signal_pretty_name + "Q"; // remove data secondary code d_data_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); d_data_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); - - interchange_iq = true; - - // used by the FPGA - d_secondary_code_length_data = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); - d_secondary_code_string_data = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); - - d_secondary_code_length_pilot = static_cast(GALILEO_E5A_Q_SECONDARY_CODE_LENGTH); - - // the data of the pilot secondary code depends on PRN and they are initialized later - - d_secondary_code_string_post_apply = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); - d_secondary_code_post_apply_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); - + // the pilot secondary code depends on PRN and it is initialized later } else { - - - //Do not acquire secondary code in data component. It is done in telemetry decoder -// d_secondary = false; - - // synchronize and remove data secondary code - d_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); - d_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); - - d_secondary_code_string_post_apply = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); - d_secondary_code_post_apply_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); - + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); signal_pretty_name = signal_pretty_name + "I"; - interchange_iq = false; - - // the coherent integration in the fpga when using E5a without tracking the pilot is not enabled - } - - - - } else { @@ -407,7 +271,6 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_symbols_per_bit = 0; @@ -419,7 +282,6 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & std::cerr << "Invalid System argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_symbols_per_bit = 0; @@ -431,9 +293,9 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & // Initialize tracking ========================================== d_code_loop_filter = Tracking_loop_filter(d_code_period, trk_parameters.dll_bw_hz, trk_parameters.dll_filter_order, false); - //printf("trk_parameters.fll_bw_hz = %f trk_parameters.pll_bw_hz = %f trk_parameters.pll_filter_order = %d\n", trk_parameters.fll_bw_hz, trk_parameters.pll_bw_hz, trk_parameters.pll_filter_order); d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_hz, trk_parameters.pll_filter_order); + // correlator outputs (scalar) if (d_veml) { // Very-Early, Early, Prompt, Late, Very-Late @@ -479,13 +341,11 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & if (trk_parameters.extend_correlation_symbols > 1) { d_enable_extended_integration = true; - //printf("d_enable_extended_integration enabled\n"); } else { d_enable_extended_integration = false; trk_parameters.extend_correlation_symbols = 1; - //printf("d_enable_extended_integration disabled\n"); } // --- Initializations --- @@ -494,47 +354,38 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_code_freq_chips = d_code_chip_rate; // Residual code phase (in chips) d_rem_code_phase_samples = 0.0; - d_rem_code_phase_samples_prev = 0.0; + d_rem_code_phase_samples_prev = 0.0; // previously calculated d_rem_code_phase_samples // Residual carrier phase d_rem_carr_phase_rad = 0.0; // sample synchronization d_sample_counter = 0ULL; d_acq_sample_stamp = 0ULL; - d_absolute_samples_offset = 0ULL; - - //d_current_prn_length_samples = static_cast(trk_parameters.vector_length); - //d_next_prn_length_samples = d_current_prn_length_samples; d_current_integration_length_samples = static_cast(trk_parameters.vector_length); d_next_integration_length_samples = d_current_integration_length_samples; - - d_extended_integration_first_prn_length_samples = static_cast(trk_parameters.vector_length); - d_extended_integration_next_prn_length_samples = d_extended_integration_first_prn_length_samples; - - //d_extended_integration_first_rem_carr_phase_rad = 0.0; - //d_extended_integration_next_rem_carr_phase_rad_step = 0.0; - - d_extended_integration_first_acc_carrier_phase_rad = 0.0; - d_extended_integration_next_acc_carrier_phase_rad_step = 0.0; - d_current_correlation_time_s = 0.0; - d_correlation_length_samples = static_cast(trk_parameters.vector_length); // this one is only for initialisation and does not change its value (MM) - // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; d_Prompt_buffer = std::vector(trk_parameters.cn0_samples); d_carrier_lock_test = 1.0; d_CN0_SNV_dB_Hz = 0.0; + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + d_carrier_lock_threshold = trk_parameters.carrier_lock_th; + d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_cn0_smoother = Exponential_Smoother(); if (d_code_period > 0.0) { d_cn0_smoother.set_samples_for_initialization(200 / static_cast(d_code_period * 1000.0)); } - d_carrier_lock_fail_counter = 0; - d_carrier_lock_threshold = trk_parameters.carrier_lock_th; - d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); + + d_carrier_lock_test_smoother = Exponential_Smoother(); + d_carrier_lock_test_smoother.set_alpha(0.002); + d_carrier_lock_test_smoother.set_min_value(-1.0); + d_carrier_lock_test_smoother.set_offset(0.0); + d_carrier_lock_test_smoother.set_samples_for_initialization(25); d_acquisition_gnss_synchro = nullptr; d_channel = 0; @@ -551,7 +402,6 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_rem_code_phase_chips = 0.0; d_state = 1; // initial state: standby clear_tracking_vars(); - if (trk_parameters.smoother_length > 0) { d_carr_ph_history.set_capacity(trk_parameters.smoother_length * 2); @@ -608,17 +458,12 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & multicorrelator_fpga->set_output_vectors(d_correlator_outs, d_Prompt_Data); d_sample_counter_next = 0ULL; - d_num_current_syncrho_repetitions = 1; - - d_corrected_doppler = false; - // debug - erase previous outfile contents - std::ofstream outfile; - outfile.open("trk_out.txt", std::ios_base::trunc); - outfile.close(); d_worker_is_done = false; + d_stop_tracking = false; + } @@ -633,7 +478,7 @@ void dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk(const pmt::pmt_t & switch (tlm_event) { - case 1: //tlm fault in current channel + case 1: // tlm fault in current channel { DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; gr::thread::scoped_lock lock(d_setlock); @@ -674,25 +519,23 @@ void dll_pll_veml_tracking_fpga::start_tracking() d_carrier_loop_filter.initialize(static_cast(d_acq_carrier_doppler_hz)); // initialize the carrier filter - d_corrected_doppler = false; boost::mutex::scoped_lock lock(d_mutex); d_worker_is_done = true; m_condition.notify_one(); - - // enable tracking pull-in - //d_state = 1; } +////bool dll_pll_veml_tracking_fpga::stop() +//void dll_pll_veml_tracking_fpga::stop() +//{ +// d_stop_tracking = true; +// //return true; +//} dll_pll_veml_tracking_fpga::~dll_pll_veml_tracking_fpga() { - if (signal_type == "1C") - { - volk_gnsssdr_free(d_preambles_symbols); - } if (d_dump_file.is_open()) { @@ -737,7 +580,6 @@ bool dll_pll_veml_tracking_fpga::acquire_secondary() for (uint32_t i = 0; i < d_secondary_code_length; i++) { if (d_Prompt_circular_buffer[i].real() < 0.0) // symbols clipping - //if (d_Prompt_buffer_deque.at(i).real() < 0.0) // symbols clipping { if (d_secondary_code_string->at(i) == '0') { @@ -772,7 +614,6 @@ bool dll_pll_veml_tracking_fpga::acquire_secondary() bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integration_time_s) { - // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### if (d_cn0_estimation_counter < trk_parameters.cn0_samples) { @@ -781,44 +622,56 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra d_cn0_estimation_counter++; return true; } - else + + d_cn0_estimation_counter = 0; + d_Prompt_buffer[d_cn0_estimation_counter++] = d_P_accu; + d_cn0_estimation_counter++; + // Code lock indicator + float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer.data(), trk_parameters.cn0_samples, static_cast(coh_integration_time_s)); + d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); + // Carrier lock indicator + //d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), trk_parameters.cn0_samples); + d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1)); + // Loss of lock detection + if (!d_pull_in_transitory) { - d_cn0_estimation_counter = 0; - // Code lock indicator - float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer.data(), trk_parameters.cn0_samples, static_cast(coh_integration_time_s)); - d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); - // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), trk_parameters.cn0_samples); - // Loss of lock detection - if (!d_pull_in_transitory) + if (d_carrier_lock_test < d_carrier_lock_threshold) { - //printf("entering d_pull_in_transitory\n"); - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < trk_parameters.cn0_min) - { - d_carrier_lock_fail_counter++; - } - else - { - if (d_carrier_lock_fail_counter > 0) - { - d_carrier_lock_fail_counter--; - } - } - } - if (d_carrier_lock_fail_counter > trk_parameters.max_lock_fail) - { - std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; - LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock - d_carrier_lock_fail_counter = 0; - multicorrelator_fpga->unlock_channel(); - return false; + d_carrier_lock_fail_counter++; } else { - return true; + if (d_carrier_lock_fail_counter > 0) + { + d_carrier_lock_fail_counter--; + } + } + + if (d_CN0_SNV_dB_Hz < trk_parameters.cn0_min) + { + d_code_lock_fail_counter++; + } + else + { + if (d_code_lock_fail_counter > 0) + { + d_code_lock_fail_counter--; + } } } + if (d_carrier_lock_fail_counter > trk_parameters.max_carrier_lock_fail or d_code_lock_fail_counter > trk_parameters.max_code_lock_fail) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel + << " (carrier_lock_fail_counter:" << d_carrier_lock_fail_counter + << " code_lock_fail_counter : " << d_code_lock_fail_counter << ")"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + multicorrelator_fpga->unlock_channel(); + return false; + } + return true; } @@ -846,7 +699,6 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() { // ################## PLL ########################################################## // PLL discriminator - //printf("d_cloop = %d\n", d_cloop); if (d_cloop) { // Costas loop discriminator, insensitive to 180 deg phase transitions @@ -861,17 +713,20 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true) or trk_parameters.enable_fll_steady_state) { // FLL discriminator - d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + // d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + d_carr_freq_error_hz = fll_diff_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + d_P_accu_old = d_P_accu; + // std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << std::endl; // Carrier discriminator filter if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true)) { - //pure FLL, disable PLL + // pure FLL, disable PLL d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, 0, d_current_correlation_time_s); } else { - //FLL-aided PLL + // FLL-aided PLL d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, d_carr_phase_error_hz, d_current_correlation_time_s); } } @@ -886,6 +741,7 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() // std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << std::endl; // std::cout << "d_CN0_SNV_dB_Hz: " << this->d_CN0_SNV_dB_Hz << std::endl; + // ################## DLL ########################################################## // DLL discriminator if (d_veml) @@ -898,7 +754,6 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() } // Code discriminator filter d_code_error_filt_chips = d_code_loop_filter.apply(d_code_error_chips); // [chips/second] - // New code Doppler frequency estimation d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate - d_code_error_filt_chips; @@ -923,7 +778,6 @@ void dll_pll_veml_tracking_fpga::run_dll_pll() } } } - } @@ -953,107 +807,19 @@ void dll_pll_veml_tracking_fpga::clear_tracking_vars() void dll_pll_veml_tracking_fpga::update_tracking_vars() { - - T_chip_seconds = 1.0 / d_code_freq_chips; // d_code_freq_chips updated in dll-pll - T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); // d_code_freq_chips fixed permanently - - // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - T_prn_samples_prev = T_prn_samples; - T_prn_samples = T_prn_seconds * trk_parameters.fs_in; - //K_blk_samples_prev = K_blk_samples; - K_blk_samples = T_prn_samples + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function - - d_actual_blk_length = static_cast(std::floor(K_blk_samples)); - d_next_integration_length_samples = 2*d_actual_blk_length - d_current_integration_length_samples; - - //################### PLL COMMANDS ################################################# - // carrier phase step (NCO phase increment per sample) [rads/sample] - d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in; - // carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2] - if (trk_parameters.high_dyn) - { - d_carr_ph_history.push_back(std::pair(d_carrier_phase_step_rad, static_cast(d_current_integration_length_samples))); - if (d_carr_ph_history.full()) - { - double tmp_cp1 = 0.0; - double tmp_cp2 = 0.0; - double tmp_samples = 0.0; - for (unsigned int k = 0; k < trk_parameters.smoother_length; k++) - { - tmp_cp1 += d_carr_ph_history[k].first; - tmp_cp2 += d_carr_ph_history[trk_parameters.smoother_length * 2 - k - 1].first; - tmp_samples += d_carr_ph_history[trk_parameters.smoother_length * 2 - k - 1].second; - } - tmp_cp1 /= static_cast(trk_parameters.smoother_length); - tmp_cp2 /= static_cast(trk_parameters.smoother_length); - d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples; - } - } - // remnant carrier phase - d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); - - // debug - debug_d_rem_carr_phase_rad = d_rem_carr_phase_rad; - - d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); - - // carrier phase accumulator - d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); - - //################### DLL COMMANDS ################################################# - // code phase step (Code resampler phase increment per sample) [chips/sample] - d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; - if (trk_parameters.high_dyn) - { - d_code_ph_history.push_back(std::pair(d_code_phase_step_chips, static_cast(d_current_integration_length_samples))); - if (d_code_ph_history.full()) - { - double tmp_cp1 = 0.0; - double tmp_cp2 = 0.0; - double tmp_samples = 0.0; - for (unsigned int k = 0; k < trk_parameters.smoother_length; k++) - { - tmp_cp1 += d_code_ph_history[k].first; - tmp_cp2 += d_code_ph_history[trk_parameters.smoother_length * 2 - k - 1].first; - tmp_samples += d_code_ph_history[trk_parameters.smoother_length * 2 - k - 1].second; - } - tmp_cp1 /= static_cast(trk_parameters.smoother_length); - tmp_cp2 /= static_cast(trk_parameters.smoother_length); - d_code_phase_rate_step_chips = (tmp_cp2 - tmp_cp1) / tmp_samples; - } - } - // remnant code phase [chips] - d_rem_code_phase_samples_prev = d_rem_code_phase_samples; - d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_integration_length_samples); // rounding error < 1 sample - d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / trk_parameters.fs_in; -} - - -void dll_pll_veml_tracking_fpga::update_tracking_vars_extend_integration_in_FPGA() -{ - // first compute the long integration intermediate prn length values based on the current values (not the values that are compute here for the next iteration) - - //d_extended_integration_first_prn_length_samples = d_current_integration_length_samples - (trk_parameters.extend_correlation_symbols - 1)*static_cast(std::floor(T_prn_samples)); - d_extended_integration_first_prn_length_samples = d_current_integration_length_samples - (d_fpga_integration_period - 1)*static_cast(std::floor(T_prn_samples)); - - d_extended_integration_next_prn_length_samples = static_cast(std::floor(T_prn_samples)); - - T_chip_seconds = 1.0 / d_code_freq_chips; + T_chip_seconds = 1.0 / d_code_freq_chips; T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### // keep alignment parameters for the next input buffer // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - T_prn_samples_prev = T_prn_samples; + //T_prn_samples_prev = T_prn_samples; T_prn_samples = T_prn_seconds * trk_parameters.fs_in; - //K_blk_samples_prev = K_blk_samples; - //K_blk_samples = T_prn_samples*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples; - K_blk_samples = T_prn_samples*d_fpga_integration_period + d_rem_code_phase_samples; + //K_blk_samples = T_prn_samples + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function + K_blk_samples = T_prn_samples*d_current_fpga_integration_period + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function - d_actual_blk_length = static_cast(std::floor(K_blk_samples)); - d_next_integration_length_samples = 2*d_actual_blk_length - d_current_integration_length_samples; + int32_t actual_blk_length = static_cast(std::floor(K_blk_samples)); + d_next_integration_length_samples = 2*actual_blk_length - d_current_integration_length_samples; //################### PLL COMMANDS ################################################# // carrier phase step (NCO phase increment per sample) [rads/sample] @@ -1078,18 +844,15 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars_extend_integration_in_FPGA d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples; } } - // remnant carrier phase + // std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; + // remnant carrier phase to prevent overflow in the code NCO d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); - - // debug - debug_d_rem_carr_phase_rad = d_rem_carr_phase_rad; - d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); - // compute the long integration intermediate phase values - d_extended_integration_first_acc_carrier_phase_rad = d_acc_carrier_phase_rad - (d_carrier_phase_step_rad * static_cast(d_extended_integration_first_prn_length_samples) - 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_extended_integration_first_prn_length_samples) * static_cast(d_extended_integration_first_prn_length_samples)); - d_extended_integration_next_acc_carrier_phase_rad_step = (d_carrier_phase_step_rad * static_cast(d_extended_integration_next_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_extended_integration_next_prn_length_samples) * static_cast(d_extended_integration_next_prn_length_samples)) - + 0.5 * 2 * d_carrier_phase_rate_step_rad * static_cast(d_extended_integration_next_prn_length_samples*d_extended_integration_first_prn_length_samples); + // carrier phase accumulator + // double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + // double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); + // std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl; d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_integration_length_samples) * static_cast(d_current_integration_length_samples)); //################### DLL COMMANDS ################################################# @@ -1118,14 +881,12 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars_extend_integration_in_FPGA d_rem_code_phase_samples_prev = d_rem_code_phase_samples; d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_integration_length_samples); // rounding error < 1 sample d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / trk_parameters.fs_in; - - } void dll_pll_veml_tracking_fpga::save_correlation_results() { - if (d_secondary) + if (d_secondary && (!d_current_extended_correlation_in_fpga)) // the FPGA removes the secondary code { if (d_secondary_code_string->at(d_current_symbol) == '0') { @@ -1163,9 +924,6 @@ void dll_pll_veml_tracking_fpga::save_correlation_results() d_E_accu += *d_Early; d_P_accu += *d_Prompt; d_L_accu += *d_Late; - - d_current_symbol++; - d_current_symbol %= d_symbols_per_bit; } // data secondary code roll-up @@ -1175,29 +933,42 @@ void dll_pll_veml_tracking_fpga::save_correlation_results() { if (trk_parameters.track_pilot) { - if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') - { - d_P_data_accu += *d_Prompt_Data; - } - else - { - d_P_data_accu -= *d_Prompt_Data; - } + if (!d_current_extended_correlation_in_fpga) // the FPGA removes the secondary code + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt_Data; + } + else + { + d_P_data_accu -= *d_Prompt_Data; + } + } + else + { + d_P_data_accu += *d_Prompt_Data; + } } else { - if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') - { - d_P_data_accu += *d_Prompt; - } - else - { - d_P_data_accu -= *d_Prompt; - } + if (!d_current_extended_correlation_in_fpga) + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt; + } + else + { + d_P_data_accu -= *d_Prompt; + } + } + else + { + d_P_data_accu += *d_Prompt; + } } - d_current_data_symbol++; - // data secondary code roll-up + d_current_data_symbol += d_current_fpga_integration_period; d_current_data_symbol %= d_data_secondary_code_length; } else @@ -1211,7 +982,7 @@ void dll_pll_veml_tracking_fpga::save_correlation_results() d_P_data_accu += *d_Prompt; //std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl; } - d_current_data_symbol++; + d_current_data_symbol += d_current_fpga_integration_period; d_current_data_symbol %= d_symbols_per_bit; } } @@ -1227,10 +998,9 @@ void dll_pll_veml_tracking_fpga::save_correlation_results() } } - - // If tracking pilot, disable Costas loop if (trk_parameters.track_pilot) { + // If tracking pilot, disable Costas loop d_cloop = false; } else @@ -1239,29 +1009,6 @@ void dll_pll_veml_tracking_fpga::save_correlation_results() } } -void dll_pll_veml_tracking_fpga::save_correlation_results_extended_integration_in_FPGA() -{ - // no need to remove the secondary code. When extended coherent integration is enabled - // the secondary code is always removed in the FPGA - if (d_veml) - { - d_VE_accu += *d_Very_Early; - d_VL_accu += *d_Very_Late; - } - d_E_accu += *d_Early; - d_P_accu += *d_Prompt; - d_L_accu += *d_Late; - - // If tracking pilot, disable Costas loop - if (trk_parameters.track_pilot) - { - d_cloop = false; - } - else - { - d_cloop = true; - } -} void dll_pll_veml_tracking_fpga::log_data() { @@ -1276,29 +1023,13 @@ void dll_pll_veml_tracking_fpga::log_data() uint64_t tmp_long_int; if (trk_parameters.track_pilot) { - if (interchange_iq) - { - prompt_I = d_Prompt_Data->imag(); - prompt_Q = d_Prompt_Data->real(); - } - else - { - prompt_I = d_Prompt_Data->real(); - prompt_Q = d_Prompt_Data->imag(); - } + prompt_I = d_Prompt_Data->real(); + prompt_Q = d_Prompt_Data->imag(); } else { - if (interchange_iq) - { - prompt_I = d_Prompt->imag(); - prompt_Q = d_Prompt->real(); - } - else - { - prompt_I = d_Prompt->real(); - prompt_Q = d_Prompt->imag(); - } + prompt_I = d_Prompt->real(); + prompt_Q = d_Prompt->imag(); } if (d_veml) { @@ -1313,20 +1044,6 @@ void dll_pll_veml_tracking_fpga::log_data() tmp_E = std::abs(d_E_accu); tmp_P = std::abs(d_P_accu); tmp_L = std::abs(d_L_accu); -// if (integrating) -// { -// //TODO: Improve this solution! -// // It compensates the amplitude difference while integrating -// if (d_extend_correlation_symbols_count > 0) -// { -// float scale_factor = static_cast(trk_parameters.extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); -// tmp_VE *= scale_factor; -// tmp_E *= scale_factor; -// tmp_P *= scale_factor; -// tmp_L *= scale_factor; -// tmp_VL *= scale_factor; -// } -// } try { @@ -1340,7 +1057,6 @@ void dll_pll_veml_tracking_fpga::log_data() d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); // PRN start sample stamp - //tmp_long_int = d_sample_counter + static_cast(d_current_integration_length_samples); tmp_long_int = d_sample_counter_next; d_dump_file.write(reinterpret_cast(&tmp_long_int), sizeof(uint64_t)); // accumulated carrier phase @@ -1359,7 +1075,6 @@ void dll_pll_veml_tracking_fpga::log_data() d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // PLL commands tmp_float = d_carr_phase_error_hz; - //tmp_float = d_carr_error_hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = d_carr_error_filt_hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); @@ -1376,7 +1091,7 @@ void dll_pll_veml_tracking_fpga::log_data() // AUX vars (for debug purposes) tmp_float = d_rem_code_phase_samples; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_integration_length_samples); + tmp_double = static_cast(d_sample_counter_next); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // PRN uint32_t prn_ = d_acquisition_gnss_synchro->PRN; @@ -1390,168 +1105,6 @@ void dll_pll_veml_tracking_fpga::log_data() } -void dll_pll_veml_tracking_fpga::log_data_extended_integration_in_FPGA(bool extended_correlation_in_fpga_enabled) -{ - if (d_dump) - { - // Dump results to file - float prompt_I; - float prompt_Q; - float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; - float tmp_float; - double tmp_double; - uint64_t tmp_long_int; - if (trk_parameters.track_pilot) - { - if (interchange_iq) - { - prompt_I = d_Prompt_Data->imag(); - prompt_Q = d_Prompt_Data->real(); - } - else - { - prompt_I = d_Prompt_Data->real(); - prompt_Q = d_Prompt_Data->imag(); - } - } - else - { - if (interchange_iq) - { - prompt_I = d_Prompt->imag(); - prompt_Q = d_Prompt->real(); - } - else - { - prompt_I = d_Prompt->real(); - prompt_Q = d_Prompt->imag(); - } - } - if (d_veml) - { - tmp_VE = std::abs(d_VE_accu); - tmp_VL = std::abs(d_VL_accu); - } - else - { - tmp_VE = 0.0; - tmp_VL = 0.0; - } - tmp_E = std::abs(d_E_accu); - tmp_P = std::abs(d_P_accu); - tmp_L = std::abs(d_L_accu); - - // no need to apply a scale factor. The - // the signal is scaled up by the longer integration time in the FPGA - //if (integrating) - if (d_extend_fpga_integration_periods > 1) - { - //TODO: Improve this solution! - // It compensates the amplitude difference while integrating - if (d_extend_correlation_symbols_count > 0) - { - float scale_factor = static_cast(trk_parameters.extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); - tmp_VE *= scale_factor; - tmp_E *= scale_factor; - tmp_P *= scale_factor; - tmp_L *= scale_factor; - tmp_VL *= scale_factor; - } - } - - - - for (uint32_t k=0;k(&tmp_VE), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); - // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); - - // PRN start sample stamp - if (!extended_correlation_in_fpga_enabled) - { - tmp_long_int = d_sample_counter_next; - } - else - { - tmp_long_int = d_sample_counter + d_extended_integration_first_prn_length_samples + d_extended_integration_next_prn_length_samples*k; - } - d_dump_file.write(reinterpret_cast(&tmp_long_int), sizeof(uint64_t)); - - // accumulated carrier phase - if (!extended_correlation_in_fpga_enabled) - { - tmp_float = d_acc_carrier_phase_rad; - - } - else - { - tmp_float = d_extended_integration_first_acc_carrier_phase_rad - d_extended_integration_next_acc_carrier_phase_rad_step*k; - } - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - - // carrier and code frequency - tmp_float = d_carrier_doppler_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // carrier phase rate [Hz/s] - tmp_float = d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_code_freq_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // code phase rate [chips/s^2] - tmp_float = d_code_phase_rate_step_chips * trk_parameters.fs_in * trk_parameters.fs_in; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // PLL commands - tmp_float = d_carr_phase_error_hz; - //tmp_float = d_carr_error_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_carr_error_filt_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // DLL commands - tmp_float = d_code_error_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_code_error_filt_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // CN0 and carrier lock test - tmp_float = d_CN0_SNV_dB_Hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_carrier_lock_test; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // AUX vars (for debug purposes) - tmp_float = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - if (!extended_correlation_in_fpga_enabled) - { - tmp_double = static_cast(d_sample_counter_next); - } - else - { - tmp_double = static_cast(d_sample_counter + d_extended_integration_first_prn_length_samples*(k+1)); - } - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // PRN - uint32_t prn_ = d_acquisition_gnss_synchro->PRN; - d_dump_file.write(reinterpret_cast(&prn_), sizeof(uint32_t)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing trk dump file " << e.what(); - } - } - } -} - - - int32_t dll_pll_veml_tracking_fpga::save_matfile() { // READ DUMP FILE @@ -1611,7 +1164,6 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile() auto aux1 = std::vector(num_epoch); auto aux2 = std::vector(num_epoch); auto PRN = std::vector(num_epoch); - try { if (dump_file.is_open()) @@ -1756,9 +1308,7 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile() void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) { d_channel = channel; - multicorrelator_fpga->set_channel(d_channel); - LOG(INFO) << "Tracking Channel set to " << d_channel; // ############# ENABLE DATA FILE LOG ################# if (d_dump) @@ -1773,6 +1323,8 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) { try { + //trk_parameters.dump_filename.append(boost::lexical_cast(d_channel)); + //trk_parameters.dump_filename.append(".dat"); d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); d_dump_file.open(dump_filename_.c_str(), std::ios::out | std::ios::binary); LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << dump_filename_.c_str(); @@ -1783,7 +1335,6 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) } } } - if (d_enable_extended_integration == true) { if (d_extended_correlation_in_fpga == true) @@ -1795,14 +1346,14 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) { if (trk_parameters.track_pilot) { - multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); - multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); - multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length, d_data_secondary_code_length); + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string); + multicorrelator_fpga->initialize_secondary_code(1, d_data_secondary_code_string); } else { - multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_data, 0); - multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_data); + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length, 0); + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string); } } } @@ -1813,16 +1364,14 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) // coherent integration in the FPGA is only enabled when tracking the pilot. if (trk_parameters.track_pilot) { - multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length_pilot, d_secondary_code_length_data); - //multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); - multicorrelator_fpga->initialize_secondary_code(1, d_secondary_code_string_data); + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length, d_data_secondary_code_length); + multicorrelator_fpga->initialize_secondary_code(1, d_data_secondary_code_string); } } } } } - } @@ -1871,20 +1420,16 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) { if (d_extended_correlation_in_fpga == true) { - // used by the FPGA - d_secondary_code_string_pilot = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); - - multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string_pilot); - + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string); } } } - } std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; d_rem_code_phase_samples = 0.0; d_rem_carr_phase_rad = 0.0; d_rem_code_phase_chips = 0.0; @@ -1922,28 +1467,8 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_Prompt_circular_buffer.clear(); - d_extended_integration_first_prn_length_samples = static_cast(trk_parameters.vector_length); - d_extended_integration_next_prn_length_samples = d_extended_integration_first_prn_length_samples; - - d_extended_integration_first_acc_carrier_phase_rad = 0.0; - d_extended_integration_next_acc_carrier_phase_rad_step = 0.0; - - d_num_current_syncrho_repetitions = 1; - - // initialize K_blk_samples_prev, T_prn_samples_prev (in the HW the frame length is corrected in the next frame, not in the current frame, - // so we need the default parameters for the first frame - T_chip_seconds = 1.0 / d_code_freq_chips; T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); - T_prn_samples_prev = T_prn_seconds * trk_parameters.fs_in; - - //K_blk_samples_prev = T_prn_samples_prev*trk_parameters.extend_correlation_symbols; - //K_blk_samples_prev = T_prn_samples_prev*d_fpga_integration_period; - d_debug_counter = 0; - - enable_post_apply_secondary_code = 0; - d_secondary_code_post_apply_counter = 0; - d_secondary_code_debug_counter_whole_bits = -1; // re-establish nominal integration length (not extended integration by default) d_current_integration_length_samples = static_cast(trk_parameters.vector_length); @@ -1951,7 +1476,8 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) multicorrelator_fpga->disable_secondary_codes(); // make sure the processing of the secondary codes is disabled by default - + d_current_fpga_integration_period = 1; + d_current_extended_correlation_in_fpga = false; } @@ -1961,14 +1487,12 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) void dll_pll_veml_tracking_fpga::stop_tracking() { - printf("stop tracking called\n"); d_state = 1; } void dll_pll_veml_tracking_fpga::reset(void) { - printf("reset and unlock channel called\n"); multicorrelator_fpga->unlock_channel(); } @@ -1982,839 +1506,513 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un Gnss_Synchro current_synchro_data = Gnss_Synchro(); current_synchro_data.Flag_valid_symbol_output = false; - d_current_integration_length_samples = d_next_integration_length_samples; + while((!current_synchro_data.Flag_valid_symbol_output) && (!d_stop_tracking)) + { - if (d_pull_in_transitory == true) - { - if (d_sample_counter > 0) // do not execute this condition until the sample counter has ben read for the first time after start_tracking - { - if (trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) - { - d_pull_in_transitory = false; - } - } - } + d_current_integration_length_samples = d_next_integration_length_samples; - bool extended_correlation_in_fpga_enabled = false; + if (d_pull_in_transitory == true) + { + if (d_sample_counter > 0) // do not execute this condition until the sample counter has ben read for the first time after start_tracking + { + if (trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) + { + d_pull_in_transitory = false; + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + } + } + } + switch (d_state) + { + case 1: // Pull-in + { - switch (d_state) - { -// case 0: // Standby - Consume samples at full throttle, do nothing -// { -// -// // EL MUTEX NO FA FALTA !! GNURADIO JA EN TE UN ARREGLAR -// printf("in state 0\n"); -// d_worker_is_done = false; -// -// -// boost::mutex::scoped_lock lock(d_mutex); -// while (!d_worker_is_done) m_condition.wait(lock); -// -// d_state = 1; -// -// //*out[0] = *d_acquisition_gnss_synchro; -// ////usleep(1000); -// //return 1; -// } - case 1: // Pull-in - { - - // EL MUTEX NO FA FALTA !! GNURADIO JA EN TE UN ARREGLAR - printf("in state 1\n"); - d_worker_is_done = false; + d_worker_is_done = false; - boost::mutex::scoped_lock lock(d_mutex); - while (!d_worker_is_done) m_condition.wait(lock); + boost::mutex::scoped_lock lock(d_mutex); + while (!d_worker_is_done) m_condition.wait(lock); - // Signal alignment (skip samples until the incoming signal is aligned with local replica) + // Signal alignment (skip samples until the incoming signal is aligned with local replica) - int64_t acq_trk_diff_samples; - double acq_trk_diff_seconds; - double delta_trk_to_acq_prn_start_samples; + int64_t acq_trk_diff_samples; + double acq_trk_diff_seconds; + double delta_trk_to_acq_prn_start_samples; - multicorrelator_fpga->lock_channel(); - uint64_t counter_value = multicorrelator_fpga->read_sample_counter(); - uint64_t absolute_samples_offset; - if (counter_value > (d_acq_sample_stamp + d_acq_code_phase_samples)) - { - // Signal alignment (skip samples until the incoming signal is aligned with local replica) - acq_trk_diff_samples = static_cast(counter_value) - static_cast(d_acq_sample_stamp); - acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; - delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) - d_acq_code_phase_samples; + multicorrelator_fpga->lock_channel(); + uint64_t counter_value = multicorrelator_fpga->read_sample_counter(); + uint64_t absolute_samples_offset; + if (counter_value > (d_acq_sample_stamp + d_acq_code_phase_samples)) + { + // Signal alignment (skip samples until the incoming signal is aligned with local replica) + acq_trk_diff_samples = static_cast(counter_value) - static_cast(d_acq_sample_stamp); + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; + delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) - d_acq_code_phase_samples; - uint32_t num_frames = ceil((delta_trk_to_acq_prn_start_samples) / d_correlation_length_samples); - absolute_samples_offset = static_cast(d_acq_code_phase_samples + d_acq_sample_stamp + num_frames * d_correlation_length_samples); - } - else - { - // test mode + uint32_t num_frames = ceil((delta_trk_to_acq_prn_start_samples) / d_current_integration_length_samples); + absolute_samples_offset = static_cast(d_acq_code_phase_samples + d_acq_sample_stamp + num_frames * d_current_integration_length_samples); + } + else + { + // test mode - acq_trk_diff_samples = -static_cast(counter_value) + static_cast(d_acq_sample_stamp); - acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; - delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) + d_acq_code_phase_samples; + acq_trk_diff_samples = -static_cast(counter_value) + static_cast(d_acq_sample_stamp); + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; + delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) + d_acq_code_phase_samples; - absolute_samples_offset = static_cast(delta_trk_to_acq_prn_start_samples); - } + absolute_samples_offset = static_cast(delta_trk_to_acq_prn_start_samples); + } - //std::cout << "TRACKING absolute_samples_offset = " << absolute_samples_offset << std::endl; + multicorrelator_fpga->set_initial_sample(absolute_samples_offset); + //d_absolute_samples_offset = absolute_samples_offset; + d_sample_counter = absolute_samples_offset; + current_synchro_data.Tracking_sample_counter = absolute_samples_offset; + d_sample_counter_next = d_sample_counter; - multicorrelator_fpga->set_initial_sample(absolute_samples_offset); - d_absolute_samples_offset = absolute_samples_offset; - d_sample_counter = absolute_samples_offset; - current_synchro_data.Tracking_sample_counter = absolute_samples_offset; - d_sample_counter_next = d_sample_counter; + // Doppler effect Fd = (C / (C + Vr)) * F + double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; + // new chip and PRN sequence periods based on acq Doppler + d_code_freq_chips = radial_velocity * d_code_chip_rate; + d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; + d_code_phase_rate_step_chips = 0.0; - // Doppler effect Fd = (C / (C + Vr)) * F - double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; - // new chip and PRN sequence periods based on acq Doppler - d_code_freq_chips = radial_velocity * d_code_chip_rate; - d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; - d_code_phase_rate_step_chips = 0.0; + d_acq_code_phase_samples = absolute_samples_offset; - d_acq_code_phase_samples = absolute_samples_offset; + d_current_integration_length_samples = trk_parameters.vector_length; - d_current_integration_length_samples = trk_parameters.vector_length; + d_next_integration_length_samples = d_current_integration_length_samples; - d_next_integration_length_samples = d_current_integration_length_samples; + int32_t samples_offset = round(d_acq_code_phase_samples); + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); + d_state = 2; + d_cn0_smoother.reset(); + d_carrier_lock_test_smoother.reset(); - int32_t samples_offset = round(d_acq_code_phase_samples); - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); - d_state = 2; - d_cn0_smoother.reset(); + LOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; + DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz + << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; - // DEBUG OUTPUT - std::cout << "Tracking of " << systemName << " " << signal_pretty_name << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - DLOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; - std::cout << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)" << std::endl; - DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz - << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; + // // DEBUG OUTPUT + // std::cout << "Tracking of " << systemName << " " << signal_pretty_name << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + // DLOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - *out[0] = *d_acquisition_gnss_synchro; - return 1; - } - case 2: // Wide tracking and symbol synchronization - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - //uint64_t d_sample_counter_tmp = d_sample_counter; - //uint64_t d_sample_counter_next_tmp = d_sample_counter_next; + // DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; + // std::cout << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)" << std::endl; + // DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz + // << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; - do_correlation_step(); + break; + } + case 2: // Wide tracking and symbol synchronization + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - // Save single correlation step variables + do_correlation_step(); - if (d_veml) - { - d_VE_accu = *d_Very_Early; - d_VL_accu = *d_Very_Late; - } - d_E_accu = *d_Early; - d_P_accu = *d_Prompt; - d_L_accu = *d_Late; + // Save single correlation step variables - //fail-safe: check if the secondary code or bit synchronization has not succedded 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)) - { - d_carrier_lock_fail_counter = 300000; //force loss-of-lock condition - LOG(INFO) << systemName << " " << signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - } + if (d_veml) + { + d_VE_accu = *d_Very_Early; + d_VL_accu = *d_Very_Late; + } + d_E_accu = *d_Early; + d_P_accu = *d_Prompt; + d_L_accu = *d_Late; - // Check lock status + //fail-safe: check if the secondary code or bit synchronization has not succedded 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)) + { + d_carrier_lock_fail_counter = 300000; //force loss-of-lock condition + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } - if (!cn0_and_tracking_lock_status(d_code_period)) - { - clear_tracking_vars(); - d_state = 1; // loss-of-lock detected - } - else - { - bool next_state = false; + // Check lock status - // Perform DLL/PLL tracking loop computations. Costas Loop enabled - run_dll_pll(); + if (!cn0_and_tracking_lock_status(d_code_period)) + { + clear_tracking_vars(); + d_state = 1; // loss-of-lock detected - update_tracking_vars(); + // send something to let the scheduler know that it has to keep on calling general work and to finish the loop + //current_synchro_data.Flag_valid_symbol_output=1; + } + else + { + bool next_state = false; - d_num_current_syncrho_repetitions = 1; + // Perform DLL/PLL tracking loop computations. Costas Loop enabled + run_dll_pll(); - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); + update_tracking_vars(); - if (!d_pull_in_transitory) - { - if (d_secondary) - { - // ####### SECONDARY CODE LOCK ##### + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + + if (!d_pull_in_transitory) + { + if (d_secondary) + { + // ####### SECONDARY CODE LOCK ##### + d_Prompt_circular_buffer.push_back(*d_Prompt); + + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); + + if (next_state) + { + LOG(INFO) << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + } + } + else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change + { + + //******* preamble correlation ******** d_Prompt_circular_buffer.push_back(*d_Prompt); - if (d_Prompt_circular_buffer.size() == d_secondary_code_length) { next_state = acquire_secondary(); - if (next_state) { - LOG(INFO) << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + std::cout << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; } } - } - else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change - { -// float current_tracking_time_s = static_cast(d_sample_counter - d_absolute_samples_offset) / trk_parameters.fs_in; -// -// if (current_tracking_time_s > 10) -// { -// d_symbol_history.push_back(d_Prompt->real()); -// -// //******* preamble correlation ******** -// -// int32_t corr_value = 0; -// if ((static_cast(d_symbol_history.size()) == d_preamble_length_symbols)) // and (d_make_correlation or !d_flag_frame_sync)) -// { -// int i = 0; -// for (const auto &iter : d_symbol_history) -// { -// if (iter < 0.0) // symbols clipping -// { -// corr_value -= d_preambles_symbols[i]; -// } -// else -// { -// corr_value += d_preambles_symbols[i]; -// } -// i++; -// } -// } -// -// if (corr_value == d_preamble_length_symbols) -// { -// LOG(INFO) << systemName << " " << signal_pretty_name << " tracking preamble detected in channel " << d_channel -// << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; -// next_state = true; -// } -// else -// { -// next_state = false; -// } -// } -// else -// { -// next_state = false; -// } - //******* preamble correlation ******** - d_Prompt_circular_buffer.push_back(*d_Prompt); - if (d_Prompt_circular_buffer.size() == d_secondary_code_length) - { - next_state = acquire_secondary(); - if (next_state) - { - LOG(INFO) << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - std::cout << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - } - } - - } + } + else + { + next_state = true; + } + } else { - next_state = true; + next_state = false; //keep in state 2 during pull-in transitory } - } - else + + if (next_state) + { // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + d_Prompt_circular_buffer.clear(); + d_current_symbol = 0; + d_current_data_symbol = 0; + + if (d_enable_extended_integration) + { + // update integration time + d_extend_correlation_symbols_count = 0; + d_current_correlation_time_s = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); + + if (d_extended_correlation_in_fpga) + { + + d_current_fpga_integration_period = d_fpga_integration_period; + d_current_extended_correlation_in_fpga = true; + + d_P_accu_old.real(d_P_accu_old.real()*d_fpga_integration_period); + d_P_accu_old.imag(d_P_accu_old.imag()*d_fpga_integration_period); + + if (d_sc_demodulate_enabled) + { + multicorrelator_fpga->enable_secondary_codes(); + } + + if (d_extend_fpga_integration_periods > 1) + { + // correction on already computed parameters + K_blk_samples = T_prn_samples*(d_fpga_integration_period) + d_rem_code_phase_samples_prev; + d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); + + d_state = 5; + } + else + { + // correction on already computed parameters + K_blk_samples = T_prn_samples*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples_prev; + d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); + + d_state = 6; + } + } + else + { + d_state = 3; // next state is the extended correlator integrator + } + + LOG(INFO) << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " + << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + std::cout << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " + << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + // Set narrow taps delay values [chips] + d_code_loop_filter.set_update_interval(d_current_correlation_time_s); + d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz); + d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order); + if (d_veml) + { + d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + 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); + } + 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); + } + } + else + { + d_state = 4; + } + } + } + + break; + } + case 3: // coherent integration (correlation time extension) + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + // perform a correlation step + do_correlation_step(); + save_correlation_results(); + update_tracking_vars(); + + if (d_current_data_symbol == 0) + { + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count == (trk_parameters.extend_correlation_symbols - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 4; + } + break; + } + case 4: // narrow tracking + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + // perform a correlation step + do_correlation_step(); + save_correlation_results(); + + // check lock status + if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) + { + clear_tracking_vars(); + d_state = 1; // loss-of-lock detected + + // send something to let the scheduler know that it has to keep on calling general work and to finish the loop + //current_synchro_data.Flag_valid_symbol_output=1; + + } + else + { + run_dll_pll(); + update_tracking_vars(); + + if (d_current_data_symbol == 0) { - next_state = false; //keep in state 2 during pull-in transitory + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); } - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } - } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - - current_synchro_data.Tracking_sample_counter = d_sample_counter_next; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - - if (next_state) - { // reset extended correlator - d_VE_accu = gr_complex(0.0, 0.0); - d_E_accu = gr_complex(0.0, 0.0); - d_P_accu = gr_complex(0.0, 0.0); - d_P_data_accu = gr_complex(0.0, 0.0); - d_L_accu = gr_complex(0.0, 0.0); - d_VL_accu = gr_complex(0.0, 0.0); - d_Prompt_circular_buffer.clear(); - d_current_symbol = 0; - d_current_data_symbol = 0; - - if (d_enable_extended_integration) - { - // update integration time - d_extend_correlation_symbols_count = 0; - d_current_correlation_time_s = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); - - if (d_extended_correlation_in_fpga) - { - - - //d_P_accu_old = d_P_accu_old*d_fpga_integration_period; - d_P_accu_old.real(d_P_accu_old.real()*d_fpga_integration_period); - d_P_accu_old.imag(d_P_accu_old.imag()*d_fpga_integration_period); - - if (d_sc_demodulate_enabled) - { - multicorrelator_fpga->enable_secondary_codes(); - } - - if (d_extend_fpga_integration_periods > 1) - { - // correction on already computed parameters - K_blk_samples = T_prn_samples*(d_fpga_integration_period) + d_rem_code_phase_samples_prev; - d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); - - d_state = 5; - } - else - { - // correction on already computed parameters - K_blk_samples = T_prn_samples*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples_prev; - d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); - - d_state = 6; - } - } - else - { - d_state = 3; // next state is the extended correlator integrator - } - - LOG(INFO) << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " - << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); - std::cout << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " - << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - // Set narrow taps delay values [chips] - d_code_loop_filter.set_update_interval(d_current_correlation_time_s); - d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz); - //std::cout << " pll_bw_narrow_hz << " << trk_parameters.pll_bw_narrow_hz << " fll_bw_hz " << trk_parameters.fll_bw_hz << std::endl; - d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order); - if (d_veml) - { - d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); - 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); - } - 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); - } - } - else - { - d_state = 4; - } - } - } - break; - } - case 3: // coherent integration (correlation time extension) - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - - // perform a correlation step - do_correlation_step(); - //update_tracking_vars(); - save_correlation_results(); - update_tracking_vars(); - - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } - } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - current_synchro_data.Tracking_sample_counter = d_sample_counter_next; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - d_extend_correlation_symbols_count++; - if (d_extend_correlation_symbols_count == (trk_parameters.extend_correlation_symbols - 1)) - { - d_extend_correlation_symbols_count = 0; - d_state = 4; - } - d_num_current_syncrho_repetitions = 1; - log_data(); - break; - } - case 4: // narrow tracking - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - - // perform a correlation step - do_correlation_step(); - save_correlation_results(); - - // check lock status - if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) - { - clear_tracking_vars(); - d_state = 1; // loss-of-lock detected - } - else - { - run_dll_pll(); - update_tracking_vars(); - - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } - } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - - current_synchro_data.Tracking_sample_counter = d_sample_counter_next; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - - - d_num_current_syncrho_repetitions = 1; - log_data(); - // reset extended correlator - d_VE_accu = gr_complex(0.0, 0.0); - d_E_accu = gr_complex(0.0, 0.0); - d_P_accu = gr_complex(0.0, 0.0); - d_L_accu = gr_complex(0.0, 0.0); - d_VL_accu = gr_complex(0.0, 0.0); - if (d_enable_extended_integration) - { - d_state = 3; // new coherent integration (correlation time extension) cycle - } - } - break; - } - - case 5: // coherent integration (correlation time extension) - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - - extended_correlation_in_fpga_enabled = true; - - // this must be computed for the secondary prn code - if (d_secondary) - { - - d_first_length_secondary_code = d_current_integration_length_samples - (d_fpga_integration_period - 1)*static_cast(std::floor(T_prn_samples)); - d_next_length_secondary_code = static_cast(std::floor(T_prn_samples)); - - multicorrelator_fpga->update_secondary_code_length(d_first_length_secondary_code, d_next_length_secondary_code); - } - - // perform a correlation step - do_correlation_step(); - //update_tracking_vars_extend_integration_in_FPGA(); - save_correlation_results_extended_integration_in_FPGA(); - update_tracking_vars_extend_integration_in_FPGA(); - - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } - } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_extended_integration_first_prn_length_samples; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_extended_integration_first_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - - d_num_current_syncrho_repetitions = d_fpga_integration_period; - - d_extend_correlation_symbols_count++; - if (d_extend_correlation_symbols_count == (d_extend_fpga_integration_periods - 1)) - { - d_extend_correlation_symbols_count = 0; - d_state = 6; - } - - - log_data_extended_integration_in_FPGA(true); - break; - } - - - case 6: // narrow tracking IN THE FPGA - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - - extended_correlation_in_fpga_enabled = true; - - // this must be computed for the secondary prn code - if (d_secondary) - { - d_first_length_secondary_code = d_current_integration_length_samples - (d_fpga_integration_period - 1)*static_cast(std::floor(T_prn_samples)); - d_next_length_secondary_code = static_cast(std::floor(T_prn_samples)); - - multicorrelator_fpga->update_secondary_code_length(d_first_length_secondary_code, d_next_length_secondary_code); - } - - // perform a correlation step - do_correlation_step(); - - save_correlation_results_extended_integration_in_FPGA(); - - // check lock status - if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) - { - clear_tracking_vars(); - d_state = 1; // loss-of-lock detected - } - else - { - run_dll_pll(); - update_tracking_vars_extend_integration_in_FPGA(); - - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } - } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_extended_integration_first_prn_length_samples; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_extended_integration_first_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - - d_num_current_syncrho_repetitions = d_fpga_integration_period; - - d_extend_correlation_symbols_count = 0; - - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data_extended_integration_in_FPGA(true); - // reset extended correlator - d_VE_accu = gr_complex(0.0, 0.0); - d_E_accu = gr_complex(0.0, 0.0); - d_P_accu = gr_complex(0.0, 0.0); - d_L_accu = gr_complex(0.0, 0.0); - d_VL_accu = gr_complex(0.0, 0.0); - - if (d_extend_fpga_integration_periods > 1) - { - d_state = 5; - } - - - } - break; - } - } - - - - - // debug - std::ofstream outfile; - - - if (d_num_current_syncrho_repetitions > 0) - { - current_synchro_data.fs = static_cast(trk_parameters.fs_in); - - // debug - d_sc_prompt_changed = false; - if (d_state == 3 || d_state == 4 || d_state == 5 || d_state ==6) - { - // debug - remodulate secondary code - if (d_sc_remodulate_enabled == true) - { - - if (enable_post_apply_secondary_code == 0) - { - enable_post_apply_secondary_code = 1; // the first time we must not yet start correcting as we are coming from state 2 - - } - else - { - if (d_secondary_code_string_post_apply->at(d_secondary_code_post_apply_counter) == '1') - { - d_sc_prompt_changed = true; - current_synchro_data.Prompt_I = - current_synchro_data.Prompt_I; - current_synchro_data.Prompt_Q = - current_synchro_data.Prompt_Q; + // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + if (d_enable_extended_integration) + { + d_state = 3; // new coherent integration (correlation time extension) cycle + } } - d_secondary_code_post_apply_counter = d_secondary_code_post_apply_counter + 1; - if (d_secondary_code_post_apply_counter == d_secondary_code_post_apply_length) - { - d_secondary_code_post_apply_counter = 0; - } - - - } - - } - - if (d_flag_printout == true) - { - outfile.open("trk_out.txt", std::ios_base::app); - outfile << d_debug_counter << " "; - - if (current_synchro_data.Prompt_I < 0) - { - outfile << "1 " << debug_d_rem_carr_phase_rad << " " << d_rem_carr_phase_rad << std::endl; - } - else - { - outfile << "0 " << debug_d_rem_carr_phase_rad << " " << d_rem_carr_phase_rad << std::endl; - } - - d_secondary_code_debug_counter_whole_bits = d_secondary_code_debug_counter_whole_bits + 1; - if (d_secondary_code_debug_counter_whole_bits == 10) - { - d_secondary_code_debug_counter_whole_bits = 0; - outfile << "--" << std::endl; - } - outfile.close(); - d_debug_counter++; - } - } - out[0][0] = current_synchro_data; - - if (d_sc_prompt_changed == true) - { - // put back to normal - current_synchro_data.Prompt_I = - current_synchro_data.Prompt_I; - current_synchro_data.Prompt_Q = - current_synchro_data.Prompt_Q; + break; } - - if (extended_correlation_in_fpga_enabled) - { - - d_sample_counter = d_sample_counter + d_extended_integration_first_prn_length_samples; - for (uint32_t k=1;k< d_num_current_syncrho_repetitions;k++) + case 5: // coherent integration (correlation time extension) { - d_sample_counter = d_sample_counter + d_extended_integration_next_prn_length_samples; - current_synchro_data.Tracking_sample_counter = d_sample_counter; - d_extended_integration_first_acc_carrier_phase_rad -= d_extended_integration_next_acc_carrier_phase_rad_step; - current_synchro_data.Carrier_phase_rads = d_extended_integration_first_acc_carrier_phase_rad; + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - d_sc_prompt_changed = false; - if (d_state == 3 || d_state == 4 || d_state == 5 || d_state ==6) - { - - if (d_sc_remodulate_enabled == true) - { - - if (d_secondary_code_string_post_apply->at(d_secondary_code_post_apply_counter) == '1') - { - d_sc_prompt_changed = true; - current_synchro_data.Prompt_I = - current_synchro_data.Prompt_I; - current_synchro_data.Prompt_Q = - current_synchro_data.Prompt_Q; - } - d_secondary_code_post_apply_counter = d_secondary_code_post_apply_counter + 1; - if (d_secondary_code_post_apply_counter == d_secondary_code_post_apply_length) - { - d_secondary_code_post_apply_counter = 0; - } - } - - if (d_flag_printout == true) - { - outfile.open("trk_out.txt", std::ios_base::app); - outfile << d_debug_counter << " "; - if (current_synchro_data.Prompt_I < 0) - { - outfile << "1 " << debug_d_rem_carr_phase_rad << " " << d_rem_carr_phase_rad << std::endl; - } - else - { - outfile << "0 " << debug_d_rem_carr_phase_rad << " " << d_rem_carr_phase_rad << std::endl; - } - - d_secondary_code_debug_counter_whole_bits = d_secondary_code_debug_counter_whole_bits + 1; - if (d_secondary_code_debug_counter_whole_bits == 10) - { - d_secondary_code_debug_counter_whole_bits = 0; - outfile << "--" << std::endl; - } - - outfile.close(); - d_debug_counter++; - } - } - out[0][k]=current_synchro_data; - if (d_sc_prompt_changed == true) + // this must be computed for the secondary prn code + if (d_secondary) { - // put back to normal - current_synchro_data.Prompt_I = - current_synchro_data.Prompt_I; - current_synchro_data.Prompt_Q = - current_synchro_data.Prompt_Q; + + uint32_t first_length_secondary_code = d_current_integration_length_samples - (d_fpga_integration_period - 1)*static_cast(std::floor(T_prn_samples)); + uint32_t next_length_secondary_code = static_cast(std::floor(T_prn_samples)); + + multicorrelator_fpga->update_secondary_code_length(first_length_secondary_code, next_length_secondary_code); } + + // perform a correlation step + do_correlation_step(); + save_correlation_results(); + update_tracking_vars(); + + if (d_current_data_symbol == 0) + { + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count == (d_extend_fpga_integration_periods - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 6; + } + + break; } - //return d_num_current_syncrho_repetitions; - } - return d_num_current_syncrho_repetitions; + case 6: // narrow tracking IN THE FPGA + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + // this must be computed for the secondary prn code + if (d_secondary) + { + uint32_t first_length_secondary_code = d_current_integration_length_samples - (d_fpga_integration_period - 1)*static_cast(std::floor(T_prn_samples)); + uint32_t next_length_secondary_code = static_cast(std::floor(T_prn_samples)); + + multicorrelator_fpga->update_secondary_code_length(first_length_secondary_code, next_length_secondary_code); + } + + // perform a correlation step + do_correlation_step(); + save_correlation_results(); + // check lock status + if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) + { + clear_tracking_vars(); + d_state = 1; // loss-of-lock detected + + // send something to let the scheduler know that it has to keep on calling general work and to finish the loop + //current_synchro_data.Flag_valid_symbol_output=1; + + } + else + { + run_dll_pll(); + update_tracking_vars(); + + if (d_current_data_symbol == 0) + { + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + + d_extend_correlation_symbols_count = 0; + + // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + + if (d_extend_fpga_integration_periods > 1) + { + d_state = 5; + } + + + } + break; + } + } + } + + if (current_synchro_data.Flag_valid_symbol_output) + { + current_synchro_data.fs = static_cast(trk_parameters.fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter_next; //d_sample_counter; + *out[0] = current_synchro_data; + return 1; } +// else +// { +// current_synchro_data = *d_acquisition_gnss_synchro; +// current_synchro_data.Flag_valid_symbol_output = false; +// *out[0] = *d_acquisition_gnss_synchro; +// return 1; +// } return 0; + + } diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 907db7dbb..424556130 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -43,11 +43,8 @@ #include // for gr_vector_int, gr_vector... #include // for pmt_t #include // for int32_t -#include // for deque #include // for string, ofstream -#include // for shared_ptr -#include -#include // for pair +#include // for pair #include class Fpga_Multicorrelator_8sc; @@ -77,27 +74,25 @@ public: void reset(void); + //bool stop(void); +// void stop(void); + + private: friend dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_); void msg_handler_telemetry_to_trk(const pmt::pmt_t &msg); dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_); - void msg_handler_preamble_index(pmt::pmt_t msg); bool cn0_and_tracking_lock_status(double coh_integration_time_s); bool acquire_secondary(); void do_correlation_step(void); void run_dll_pll(); void update_tracking_vars(); - void update_tracking_vars_extend_integration_in_FPGA(); void clear_tracking_vars(); void save_correlation_results(); - void save_correlation_results_extended_integration_in_FPGA(); void log_data(); - void log_data_extended_integration_in_FPGA(bool extended_correlation_in_fpga_enabled); int32_t save_matfile(); - //void run_state_2(Gnss_Synchro ¤t_synchro_data); - // tracking configuration vars Dll_Pll_Conf_Fpga trk_parameters; bool d_veml; @@ -105,9 +100,8 @@ private: uint32_t d_channel; Gnss_Synchro *d_acquisition_gnss_synchro; - //Signal parameters + // Signal parameters bool d_secondary; - bool interchange_iq; double d_signal_carrier_freq; double d_code_period; double d_code_chip_rate; @@ -122,9 +116,6 @@ private: std::string *d_data_secondary_code_string; std::string signal_pretty_name; - int32_t *d_preambles_symbols; - int32_t d_preamble_length_symbols; - boost::circular_buffer d_symbol_history; // dll filter buffer boost::circular_buffer d_dll_filt_history; @@ -150,7 +141,6 @@ private: int32_t d_current_symbol; int32_t d_current_data_symbol; - gr_complex d_VE_accu; gr_complex d_E_accu; gr_complex d_P_accu; @@ -158,9 +148,6 @@ private: gr_complex d_L_accu; gr_complex d_VL_accu; -// gr_complex d_P_data_accu; // when the extended integration is done in the FPGA we need to accumulate the pilot correlator results too - uint32_t d_num_current_syncrho_repetitions; - gr_complex d_P_data_accu; gr_complex *d_Prompt_Data; @@ -170,14 +157,12 @@ private: double d_carrier_phase_step_rad; double d_carrier_phase_rate_step_rad; boost::circular_buffer> d_carr_ph_history; + // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; - double d_rem_code_phase_samples_first; - double d_rem_code_phase_samples_next; double d_rem_code_phase_samples_prev; float d_rem_carr_phase_rad; - // PLL and DLL filter library Tracking_loop_filter d_code_loop_filter; Tracking_FLL_PLL_filter d_carrier_loop_filter; @@ -202,98 +187,44 @@ private: double T_prn_seconds; double T_prn_samples; double K_blk_samples; - - //double K_blk_samples_prev; - + // integration period in samples int32_t d_current_integration_length_samples; - // processing samples counters uint64_t d_sample_counter; uint64_t d_acq_sample_stamp; - uint64_t d_absolute_samples_offset; - // CN0 estimation and lock detector int32_t d_cn0_estimation_counter; int32_t d_carrier_lock_fail_counter; - //std::deque d_carrier_lock_detector_queue; + int32_t d_code_lock_fail_counter; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; boost::circular_buffer d_Prompt_circular_buffer; - //std::deque d_Prompt_buffer_deque; std::vector d_Prompt_buffer; Exponential_Smoother d_cn0_smoother; - - - bool d_extended_correlation_in_fpga; - + Exponential_Smoother d_carrier_lock_test_smoother; // file dump std::ofstream d_dump_file; std::string d_dump_filename; bool d_dump; bool d_dump_mat; - // extra - int32_t d_correlation_length_samples; + bool d_extended_correlation_in_fpga; + bool d_current_extended_correlation_in_fpga; int32_t d_next_integration_length_samples; - - int32_t d_extended_integration_first_prn_length_samples; - int32_t d_extended_integration_next_prn_length_samples; - double d_extended_integration_first_acc_carrier_phase_rad; double d_extended_integration_next_acc_carrier_phase_rad_step; - uint64_t d_sample_counter_next; - - // DEBUG STUFF - uint64_t d_current_synchro_data_Tracking_sample_counter[20]; - double d_current_synchro_data_Code_phase_samples[20]; - double d_current_synchro_data_Carrier_phase_rads[20]; - double d_current_synchro_data_Carrier_Doppler_hz[20]; - double d_current_synchro_data_CN0_dB_hz[20]; - bool d_current_synchro_data_Flag_valid_symbol_output[20]; - int32_t d_current_synchro_data_correlation_length_ms[20]; - double d_current_synchro_data_Prompt_I[20]; - double d_current_synchro_data_Prompt_Q[20]; - - double T_prn_samples_prev; - int32_t d_actual_blk_length; - - bool d_flag_printout; - - std::string *d_secondary_code_string_data; - std::string *d_secondary_code_string_pilot; - - uint32_t d_secondary_code_length_data; - uint32_t d_secondary_code_length_pilot; - - uint32_t d_first_length_secondary_code; - uint32_t d_next_length_secondary_code; - - uint32_t d_debug_counter; - - uint32_t d_secondary_code_post_apply_counter; // init in set_gnss_synchro - std::string *d_secondary_code_string_post_apply; // init in constructor - uint32_t d_secondary_code_post_apply_length; // init in constructor - bool enable_post_apply_secondary_code; - uint32_t d_secondary_code_debug_counter_whole_bits; - bool d_sc_remodulate_enabled; bool d_sc_demodulate_enabled; - bool d_sc_prompt_changed; - - float debug_d_rem_carr_phase_rad; - uint32_t debug_first_time; - - uint32_t d_extend_fpga_integration_periods; - + int32_t d_extend_fpga_integration_periods; uint32_t d_fpga_integration_period; - + uint32_t d_current_fpga_integration_period; bool d_worker_is_done; - boost::condition_variable m_condition; boost::mutex d_mutex; + bool d_stop_tracking; }; diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc index 74be48fda..df5671182 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc @@ -32,6 +32,7 @@ #include "dll_pll_conf_fpga.h" +#include "gnss_sdr_flags.h" #include Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() @@ -63,10 +64,12 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() early_late_space_narrow_chips = 0.1; very_early_late_space_narrow_chips = 0.1; extend_correlation_symbols = 5; - cn0_samples = 20; - cn0_min = 25; - max_lock_fail = 50; - carrier_lock_th = 0.85; + cn0_samples = FLAGS_cn0_samples; + cn0_min = FLAGS_cn0_min; + max_carrier_lock_fail = FLAGS_max_carrier_lock_fail; + max_code_lock_fail = FLAGS_max_lock_fail; + carrier_lock_th = FLAGS_carrier_lock_th; + //max_lock_fail = 50; enable_doppler_correction = false; track_pilot = false; system = 'G'; diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h index 48acbe624..a88d80212 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h @@ -42,6 +42,7 @@ class Dll_Pll_Conf_Fpga { public: /* DLL/PLL tracking configuration */ + int fll_filter_order; bool enable_fll_pull_in; bool enable_fll_steady_state; @@ -50,7 +51,6 @@ public: int pll_filter_order; int dll_filter_order; - double fs_in; uint32_t vector_length; bool dump; @@ -71,7 +71,9 @@ public: bool high_dyn; int32_t cn0_samples; int32_t cn0_min; - int32_t max_lock_fail; + int32_t max_code_lock_fail; + int32_t max_carrier_lock_fail; + //int32_t max_lock_fail; uint32_t smoother_length; double carrier_lock_th; bool track_pilot; From 8c84702f27518c2cf8ec6929236a5262d157b922 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Thu, 18 Jul 2019 19:12:17 +0200 Subject: [PATCH 11/22] implemented the stop_tracking() function --- .../galileo_e1_dll_pll_veml_tracking_fpga.cc | 1 + .../galileo_e5a_dll_pll_tracking_fpga.cc | 1 + .../adapters/gps_l1_ca_dll_pll_tracking_fpga.cc | 1 + .../adapters/gps_l5_dll_pll_tracking_fpga.cc | 1 + .../dll_pll_veml_tracking_fpga.cc | 16 +--------------- .../gnuradio_blocks/dll_pll_veml_tracking_fpga.h | 4 ---- 6 files changed, 5 insertions(+), 19 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc index dbbf014df..fc8f0e712 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc @@ -286,6 +286,7 @@ GalileoE1DllPllVemlTrackingFpga::~GalileoE1DllPllVemlTrackingFpga() void GalileoE1DllPllVemlTrackingFpga::stop_tracking() { + tracking_fpga_sc->stop_tracking(); } diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index a6e3014f1..2b681d7ed 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -304,6 +304,7 @@ void GalileoE5aDllPllTrackingFpga::start_tracking() void GalileoE5aDllPllTrackingFpga::stop_tracking() { + tracking_fpga_sc->stop_tracking(); } diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index 97e075c13..254c0471c 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -252,6 +252,7 @@ void GpsL1CaDllPllTrackingFpga::start_tracking() void GpsL1CaDllPllTrackingFpga::stop_tracking() { + tracking_fpga_sc->stop_tracking(); } diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index e830518cb..b2d13c49a 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -327,6 +327,7 @@ void GpsL5DllPllTrackingFpga::start_tracking() void GpsL5DllPllTrackingFpga::stop_tracking() { + tracking_fpga_sc->stop_tracking(); } 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 196eba014..3b494c939 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 @@ -527,13 +527,6 @@ void dll_pll_veml_tracking_fpga::start_tracking() } -////bool dll_pll_veml_tracking_fpga::stop() -//void dll_pll_veml_tracking_fpga::stop() -//{ -// d_stop_tracking = true; -// //return true; -//} - dll_pll_veml_tracking_fpga::~dll_pll_veml_tracking_fpga() { @@ -1487,7 +1480,7 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) void dll_pll_veml_tracking_fpga::stop_tracking() { - d_state = 1; + d_stop_tracking = true; } @@ -2005,13 +1998,6 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un *out[0] = current_synchro_data; return 1; } -// else -// { -// current_synchro_data = *d_acquisition_gnss_synchro; -// current_synchro_data.Flag_valid_symbol_output = false; -// *out[0] = *d_acquisition_gnss_synchro; -// return 1; -// } return 0; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 424556130..3c3e2a203 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -74,10 +74,6 @@ public: void reset(void); - //bool stop(void); -// void stop(void); - - private: friend dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_); void msg_handler_telemetry_to_trk(const pmt::pmt_t &msg); From 8b0e170a5650a765ead8551410aa22a3977ba963 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Fri, 19 Jul 2019 11:55:53 +0200 Subject: [PATCH 12/22] applied clang-format --- .../gnuradio_blocks/pcps_acquisition_fpga.cc | 114 +- .../acquisition/libs/fpga_acquisition.cc | 2 +- .../galileo_e1_dll_pll_veml_tracking_fpga.cc | 16 +- .../galileo_e5a_dll_pll_tracking_fpga.cc | 68 +- .../gps_l1_ca_dll_pll_tracking_fpga.cc | 32 +- .../gps_l2_m_dll_pll_tracking_fpga.cc | 12 +- .../adapters/gps_l5_dll_pll_tracking_fpga.cc | 56 +- .../dll_pll_veml_tracking_fpga.cc | 1199 ++++++++--------- .../dll_pll_veml_tracking_fpga.h | 3 +- .../tracking/libs/fpga_multicorrelator.cc | 323 +++-- .../tracking/libs/fpga_multicorrelator.h | 20 +- .../gps_l1_ca_dll_pll_tracking_test_fpga.cc | 4 +- 12 files changed, 903 insertions(+), 946 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc index 3663c3ca1..4d0a10703 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc @@ -134,56 +134,52 @@ void pcps_acquisition_fpga::set_state(int32_t state) void pcps_acquisition_fpga::send_positive_acquisition() { + // debug L5 + // d_gnss_synchro->Acq_delay_samples = 2694; + // d_gnss_synchro->Acq_doppler_hz = 2650; + // d_gnss_synchro->Acq_samplestamp_samples = 56500224; + // d_gnss_synchro->Flag_valid_word = 0; + // d_gnss_synchro->Flag_valid_pseudorange = 0; + // d_gnss_synchro->Flag_valid_symbol_output = 0; + // d_gnss_synchro->Flag_valid_acquisition = 0; - // debug L5 -// d_gnss_synchro->Acq_delay_samples = 2694; -// d_gnss_synchro->Acq_doppler_hz = 2650; -// d_gnss_synchro->Acq_samplestamp_samples = 56500224; -// d_gnss_synchro->Flag_valid_word = 0; -// d_gnss_synchro->Flag_valid_pseudorange = 0; -// d_gnss_synchro->Flag_valid_symbol_output = 0; -// d_gnss_synchro->Flag_valid_acquisition = 0; + // d_gnss_synchro->Acq_delay_samples = 10846; + // d_gnss_synchro->Acq_doppler_hz = 2575; + // d_gnss_synchro->Acq_samplestamp_samples = 399605760; + // d_gnss_synchro->Flag_valid_word = 0; + // d_gnss_synchro->Flag_valid_pseudorange = 0; + // d_gnss_synchro->Flag_valid_symbol_output = 0; + // d_gnss_synchro->Flag_valid_acquisition = 0; -// d_gnss_synchro->Acq_delay_samples = 10846; -// d_gnss_synchro->Acq_doppler_hz = 2575; -// d_gnss_synchro->Acq_samplestamp_samples = 399605760; -// d_gnss_synchro->Flag_valid_word = 0; -// d_gnss_synchro->Flag_valid_pseudorange = 0; -// d_gnss_synchro->Flag_valid_symbol_output = 0; -// d_gnss_synchro->Flag_valid_acquisition = 0; + // if (d_channel == 0) + // { + // d_gnss_synchro->Acq_delay_samples = 401; + // d_gnss_synchro->Acq_doppler_hz = 2650; + // d_gnss_synchro->Acq_samplestamp_samples = 96591872; + // d_gnss_synchro->Flag_valid_word = 0; + // d_gnss_synchro->Flag_valid_pseudorange = 0; + // d_gnss_synchro->Flag_valid_symbol_output = 0; + // d_gnss_synchro->Flag_valid_acquisition = 0; -// if (d_channel == 0) -// { -// d_gnss_synchro->Acq_delay_samples = 401; -// d_gnss_synchro->Acq_doppler_hz = 2650; -// d_gnss_synchro->Acq_samplestamp_samples = 96591872; -// d_gnss_synchro->Flag_valid_word = 0; -// d_gnss_synchro->Flag_valid_pseudorange = 0; -// d_gnss_synchro->Flag_valid_symbol_output = 0; -// d_gnss_synchro->Flag_valid_acquisition = 0; - -// d_gnss_synchro->Acq_delay_samples = 1505; -// d_gnss_synchro->Acq_doppler_hz = 2575; -// d_gnss_synchro->Acq_samplestamp_samples = 194265553; -// d_gnss_synchro->Flag_valid_word = 0; -// d_gnss_synchro->Flag_valid_pseudorange = 0; -// d_gnss_synchro->Flag_valid_symbol_output = 0; -// d_gnss_synchro->Flag_valid_acquisition = 0; - -// } - - - // debug E5a -// d_gnss_synchro->Acq_delay_samples = 2012; -// d_gnss_synchro->Acq_doppler_hz = -1125; -// d_gnss_synchro->Acq_samplestamp_samples = 363462656; -// d_gnss_synchro->Flag_valid_word = 0; -// d_gnss_synchro->Flag_valid_pseudorange = 0; -// d_gnss_synchro->Flag_valid_symbol_output = 0; -// d_gnss_synchro->Flag_valid_acquisition = 0; + // d_gnss_synchro->Acq_delay_samples = 1505; + // d_gnss_synchro->Acq_doppler_hz = 2575; + // d_gnss_synchro->Acq_samplestamp_samples = 194265553; + // d_gnss_synchro->Flag_valid_word = 0; + // d_gnss_synchro->Flag_valid_pseudorange = 0; + // d_gnss_synchro->Flag_valid_symbol_output = 0; + // d_gnss_synchro->Flag_valid_acquisition = 0; + // } + // debug E5a + // d_gnss_synchro->Acq_delay_samples = 2012; + // d_gnss_synchro->Acq_doppler_hz = -1125; + // d_gnss_synchro->Acq_samplestamp_samples = 363462656; + // d_gnss_synchro->Flag_valid_word = 0; + // d_gnss_synchro->Flag_valid_pseudorange = 0; + // d_gnss_synchro->Flag_valid_symbol_output = 0; + // d_gnss_synchro->Flag_valid_acquisition = 0; // Declare positive acquisition using a message port @@ -199,21 +195,21 @@ void pcps_acquisition_fpga::send_positive_acquisition() << ", input signal power " << d_input_power; -// std::cout << "positive acquisition" -// << ", satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN -// << ", sample_stamp " << d_sample_counter -// << ", test statistics value " << d_test_statistics -// << ", test statistics threshold " << d_threshold -// << ", code phase " << d_gnss_synchro->Acq_delay_samples -// << ", doppler " << d_gnss_synchro->Acq_doppler_hz -// << ", magnitude " << d_mag -// << ", input signal power " << d_input_power -// << ", d_gnss_synchro->Acq_samplestamp_samples " << d_gnss_synchro->Acq_samplestamp_samples -// << ", d_gnss_synchro->Flag_valid_word " << d_gnss_synchro->Flag_valid_word -// << ", Flag_valid_pseudorange " << d_gnss_synchro->Flag_valid_pseudorange -// << ", d_gnss_synchro->Flag_valid_symbol_output " << d_gnss_synchro->Flag_valid_symbol_output -// << ", d_gnss_synchro->Flag_valid_acquisition " << d_gnss_synchro->Flag_valid_acquisition -// << std::endl; + // std::cout << "positive acquisition" + // << ", satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN + // << ", sample_stamp " << d_sample_counter + // << ", test statistics value " << d_test_statistics + // << ", test statistics threshold " << d_threshold + // << ", code phase " << d_gnss_synchro->Acq_delay_samples + // << ", doppler " << d_gnss_synchro->Acq_doppler_hz + // << ", magnitude " << d_mag + // << ", input signal power " << d_input_power + // << ", d_gnss_synchro->Acq_samplestamp_samples " << d_gnss_synchro->Acq_samplestamp_samples + // << ", d_gnss_synchro->Flag_valid_word " << d_gnss_synchro->Flag_valid_word + // << ", Flag_valid_pseudorange " << d_gnss_synchro->Flag_valid_pseudorange + // << ", d_gnss_synchro->Flag_valid_symbol_output " << d_gnss_synchro->Flag_valid_symbol_output + // << ", d_gnss_synchro->Flag_valid_acquisition " << d_gnss_synchro->Flag_valid_acquisition + // << std::endl; //the channel FSM is set, so, notify it directly the positive acquisition to minimize delays d_channel_fsm.lock()->Event_valid_acquisition(); diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.cc b/src/algorithms/acquisition/libs/fpga_acquisition.cc index 0713583ed..b435d29fb 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.cc +++ b/src/algorithms/acquisition/libs/fpga_acquisition.cc @@ -302,7 +302,7 @@ void Fpga_Acquisition::close_device() void Fpga_Acquisition::reset_acquisition(void) { - //printf("============ resetting the hw now from the acquisition ==============="); + //printf("============ resetting the hw now from the acquisition ==============="); d_map_base[8] = RESET_ACQUISITION; // writing a 2 to d_map_base[8] resets the acquisition. This causes a reset of all // the FPGA HW modules including the multicorrelators } diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc index fc8f0e712..622e09dcf 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc @@ -175,12 +175,12 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); -// int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); -// if (FLAGS_max_lock_fail != 50) -// { -// max_lock_fail = FLAGS_max_lock_fail; -// } -// trk_param_fpga.max_lock_fail = max_lock_fail; + // int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); + // if (FLAGS_max_lock_fail != 50) + // { + // max_lock_fail = FLAGS_max_lock_fail; + // } + // trk_param_fpga.max_lock_fail = max_lock_fail; // FPGA configuration parameters @@ -265,8 +265,8 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( trk_param_fpga.code_length_chips = GALILEO_E1_B_CODE_LENGTH_CHIPS; trk_param_fpga.code_samples_per_chip = code_samples_per_chip; // 2 sample per chip trk_param_fpga.extended_correlation_in_fpga = false; - trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) - trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) + trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) + trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) //################# MAKE TRACKING GNURadio object ################### tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index 2b681d7ed..918750cf1 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -168,12 +168,12 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); -// int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); -// if (FLAGS_max_lock_fail != 50) -// { -// max_lock_fail = FLAGS_max_lock_fail; -// } -// trk_param_fpga.max_lock_fail = max_lock_fail; + // int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); + // if (FLAGS_max_lock_fail != 50) + // { + // max_lock_fail = FLAGS_max_lock_fail; + // } + // trk_param_fpga.max_lock_fail = max_lock_fail; d_data_codes = nullptr; @@ -248,35 +248,35 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( trk_param_fpga.code_length_chips = code_length_chips; trk_param_fpga.code_samples_per_chip = code_samples_per_chip; // 2 sample per chip - trk_param_fpga.extended_correlation_in_fpga = false; // by default - trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) - trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) + trk_param_fpga.extended_correlation_in_fpga = false; // by default + trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) + trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) if (d_track_pilot) - { - if (extend_correlation_symbols >1) - { - if (extend_correlation_symbols <= GALILEO_E5A_I_SECONDARY_CODE_LENGTH) - { - if ((GALILEO_E5A_I_SECONDARY_CODE_LENGTH % extend_correlation_symbols) == 0) - { - trk_param_fpga.extended_correlation_in_fpga = true; - trk_param_fpga.fpga_integration_period = extend_correlation_symbols; - printf("correlation in fpga true\n"); - } - } - else - { - if (extend_correlation_symbols % GALILEO_E5A_I_SECONDARY_CODE_LENGTH == 0) - { - trk_param_fpga.extended_correlation_in_fpga = true; - trk_param_fpga.extend_fpga_integration_periods = extend_correlation_symbols/GALILEO_E5A_I_SECONDARY_CODE_LENGTH; - trk_param_fpga.fpga_integration_period = GALILEO_E5A_I_SECONDARY_CODE_LENGTH; - printf("correlation in fpga true\n"); - printf("extend fpga integration periods true\n"); - } - } - } - } + { + if (extend_correlation_symbols > 1) + { + if (extend_correlation_symbols <= GALILEO_E5A_I_SECONDARY_CODE_LENGTH) + { + if ((GALILEO_E5A_I_SECONDARY_CODE_LENGTH % extend_correlation_symbols) == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.fpga_integration_period = extend_correlation_symbols; + printf("correlation in fpga true\n"); + } + } + else + { + if (extend_correlation_symbols % GALILEO_E5A_I_SECONDARY_CODE_LENGTH == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.extend_fpga_integration_periods = extend_correlation_symbols / GALILEO_E5A_I_SECONDARY_CODE_LENGTH; + trk_param_fpga.fpga_integration_period = GALILEO_E5A_I_SECONDARY_CODE_LENGTH; + printf("correlation in fpga true\n"); + printf("extend fpga integration periods true\n"); + } + } + } + } //################# MAKE TRACKING GNURadio object ################### tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index 254c0471c..e3cc0b964 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -152,7 +152,7 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( } else if (symbols_extended_correlator > GPS_CA_BIT_DURATION_MS) { - symbols_extended_correlator = GPS_CA_BIT_DURATION_MS; + symbols_extended_correlator = GPS_CA_BIT_DURATION_MS; std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be lower than 21. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << std::endl; } trk_param_fpga.extend_correlation_symbols = symbols_extended_correlator; @@ -214,21 +214,21 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( trk_param_fpga.code_length_chips = GPS_L1_CA_CODE_LENGTH_CHIPS; trk_param_fpga.code_samples_per_chip = 1; // 1 sample per chip - trk_param_fpga.extended_correlation_in_fpga = false; // by default - trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) - trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) - if (symbols_extended_correlator >1) - { - if (symbols_extended_correlator <= GPS_CA_BIT_DURATION_MS) - { - if ((GPS_CA_BIT_DURATION_MS % symbols_extended_correlator) == 0) - { - trk_param_fpga.extended_correlation_in_fpga = true; - trk_param_fpga.fpga_integration_period = symbols_extended_correlator; - printf("correlation in fpga true\n"); - } - } - } + trk_param_fpga.extended_correlation_in_fpga = false; // by default + trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) + trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) + if (symbols_extended_correlator > 1) + { + if (symbols_extended_correlator <= GPS_CA_BIT_DURATION_MS) + { + if ((GPS_CA_BIT_DURATION_MS % symbols_extended_correlator) == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.fpga_integration_period = symbols_extended_correlator; + printf("correlation in fpga true\n"); + } + } + } //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc index c40ec5b93..f633c84b2 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc @@ -106,12 +106,12 @@ GpsL2MDllPllTrackingFpga::GpsL2MDllPllTrackingFpga( trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); -// int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); -// if (FLAGS_max_lock_fail != 50) -// { -// max_lock_fail = FLAGS_max_lock_fail; -// } -// trk_param_fpga.max_lock_fail = max_lock_fail; + // int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); + // if (FLAGS_max_lock_fail != 50) + // { + // max_lock_fail = FLAGS_max_lock_fail; + // } + // trk_param_fpga.max_lock_fail = max_lock_fail; // FPGA configuration parameters std::string default_device_name = "/dev/uio"; diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index b2d13c49a..73903399f 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -273,35 +273,35 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( trk_param_fpga.code_length_chips = code_length_chips; trk_param_fpga.code_samples_per_chip = code_samples_per_chip; // 2 sample per chip - trk_param_fpga.extended_correlation_in_fpga = false; // by default - trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) - trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) + trk_param_fpga.extended_correlation_in_fpga = false; // by default + trk_param_fpga.extend_fpga_integration_periods = 1; // (number of FPGA integrations that are combined in the SW) + trk_param_fpga.fpga_integration_period = 1; // (number of symbols that are effectively integrated in the FPGA) if (d_track_pilot) - { - if (extend_correlation_symbols > 1) - { - if (extend_correlation_symbols <= GPS_L5I_NH_CODE_LENGTH) - { - if ((GPS_L5I_NH_CODE_LENGTH % extend_correlation_symbols) == 0) - { - trk_param_fpga.extended_correlation_in_fpga = true; - trk_param_fpga.fpga_integration_period = extend_correlation_symbols; - printf("correlation in fpga true\n"); - } - } - else - { - if (extend_correlation_symbols % GPS_L5I_NH_CODE_LENGTH == 0) - { - trk_param_fpga.extended_correlation_in_fpga = true; - trk_param_fpga.extend_fpga_integration_periods = extend_correlation_symbols/GPS_L5I_NH_CODE_LENGTH; - trk_param_fpga.fpga_integration_period = GPS_L5I_NH_CODE_LENGTH; - printf("correlation in fpga true\n"); - printf("extend fpga integration periods true\n"); - } - } - } - } + { + if (extend_correlation_symbols > 1) + { + if (extend_correlation_symbols <= GPS_L5I_NH_CODE_LENGTH) + { + if ((GPS_L5I_NH_CODE_LENGTH % extend_correlation_symbols) == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.fpga_integration_period = extend_correlation_symbols; + printf("correlation in fpga true\n"); + } + } + else + { + if (extend_correlation_symbols % GPS_L5I_NH_CODE_LENGTH == 0) + { + trk_param_fpga.extended_correlation_in_fpga = true; + trk_param_fpga.extend_fpga_integration_periods = extend_correlation_symbols / GPS_L5I_NH_CODE_LENGTH; + trk_param_fpga.fpga_integration_period = GPS_L5I_NH_CODE_LENGTH; + printf("correlation in fpga true\n"); + printf("extend fpga integration periods true\n"); + } + } + } + } tracking_fpga_sc = dll_pll_veml_make_tracking_fpga(trk_param_fpga); channel_ = 0; 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 3b494c939..2a2ec239c 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 @@ -121,8 +121,8 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_code_length_chips = trk_parameters.code_length_chips; d_extended_correlation_in_fpga = trk_parameters.extended_correlation_in_fpga; d_current_extended_correlation_in_fpga = false; - d_extend_fpga_integration_periods = trk_parameters.extend_fpga_integration_periods; // by default - d_fpga_integration_period = trk_parameters.fpga_integration_period; // by default + d_extend_fpga_integration_periods = trk_parameters.extend_fpga_integration_periods; // by default + d_fpga_integration_period = trk_parameters.fpga_integration_period; // by default d_current_fpga_integration_period = 1; d_sc_demodulate_enabled = false; @@ -165,14 +165,13 @@ 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; - if (d_extended_correlation_in_fpga == true) - { - if (trk_parameters.extend_correlation_symbols > 1) - { - d_sc_demodulate_enabled = true; - - } - } + if (d_extended_correlation_in_fpga == true) + { + if (trk_parameters.extend_correlation_symbols > 1) + { + d_sc_demodulate_enabled = true; + } + } if (trk_parameters.track_pilot) { // synchronize pilot secondary code @@ -238,13 +237,13 @@ 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; - if (d_extended_correlation_in_fpga == true) - { - if (trk_parameters.extend_correlation_symbols > 1) - { - d_sc_demodulate_enabled = true; - } - } + if (d_extended_correlation_in_fpga == true) + { + if (trk_parameters.extend_correlation_symbols > 1) + { + d_sc_demodulate_enabled = true; + } + } if (trk_parameters.track_pilot) { // synchronize pilot secondary code @@ -255,7 +254,6 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_data_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); // the pilot secondary code depends on PRN and it is initialized later - } else { @@ -354,7 +352,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_code_freq_chips = d_code_chip_rate; // Residual code phase (in chips) d_rem_code_phase_samples = 0.0; - d_rem_code_phase_samples_prev = 0.0; // previously calculated d_rem_code_phase_samples + d_rem_code_phase_samples_prev = 0.0; // previously calculated d_rem_code_phase_samples // Residual carrier phase d_rem_carr_phase_rad = 0.0; @@ -463,8 +461,6 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_worker_is_done = false; d_stop_tracking = false; - - } void dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) @@ -524,12 +520,10 @@ void dll_pll_veml_tracking_fpga::start_tracking() boost::mutex::scoped_lock lock(d_mutex); d_worker_is_done = true; m_condition.notify_one(); - } dll_pll_veml_tracking_fpga::~dll_pll_veml_tracking_fpga() { - if (d_dump_file.is_open()) { try @@ -684,7 +678,7 @@ void dll_pll_veml_tracking_fpga::do_correlation_step(void) static_cast(d_rem_code_phase_chips) * static_cast(d_code_samples_per_chip), static_cast(d_code_phase_step_chips) * static_cast(d_code_samples_per_chip), static_cast(d_code_phase_rate_step_chips) * static_cast(d_code_samples_per_chip), - d_current_integration_length_samples); + d_current_integration_length_samples); } @@ -809,10 +803,10 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() //T_prn_samples_prev = T_prn_samples; T_prn_samples = T_prn_seconds * trk_parameters.fs_in; //K_blk_samples = T_prn_samples + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function - K_blk_samples = T_prn_samples*d_current_fpga_integration_period + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function + K_blk_samples = T_prn_samples * d_current_fpga_integration_period + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function int32_t actual_blk_length = static_cast(std::floor(K_blk_samples)); - d_next_integration_length_samples = 2*actual_blk_length - d_current_integration_length_samples; + d_next_integration_length_samples = 2 * actual_blk_length - d_current_integration_length_samples; //################### PLL COMMANDS ################################################# // carrier phase step (NCO phase increment per sample) [rads/sample] @@ -879,7 +873,7 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() void dll_pll_veml_tracking_fpga::save_correlation_results() { - if (d_secondary && (!d_current_extended_correlation_in_fpga)) // the FPGA removes the secondary code + if (d_secondary && (!d_current_extended_correlation_in_fpga)) // the FPGA removes the secondary code { if (d_secondary_code_string->at(d_current_symbol) == '0') { @@ -926,39 +920,39 @@ void dll_pll_veml_tracking_fpga::save_correlation_results() { if (trk_parameters.track_pilot) { - if (!d_current_extended_correlation_in_fpga) // the FPGA removes the secondary code - { - if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') - { - d_P_data_accu += *d_Prompt_Data; - } - else - { - d_P_data_accu -= *d_Prompt_Data; - } - } - else - { - d_P_data_accu += *d_Prompt_Data; - } + if (!d_current_extended_correlation_in_fpga) // the FPGA removes the secondary code + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt_Data; + } + else + { + d_P_data_accu -= *d_Prompt_Data; + } + } + else + { + d_P_data_accu += *d_Prompt_Data; + } } else { - if (!d_current_extended_correlation_in_fpga) - { - if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') - { - d_P_data_accu += *d_Prompt; - } - else - { - d_P_data_accu -= *d_Prompt; - } - } - else - { - d_P_data_accu += *d_Prompt; - } + if (!d_current_extended_correlation_in_fpga) + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt; + } + else + { + d_P_data_accu -= *d_Prompt; + } + } + else + { + d_P_data_accu += *d_Prompt; + } } d_current_data_symbol += d_current_fpga_integration_period; @@ -1328,43 +1322,42 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) } } } - if (d_enable_extended_integration == true) - { - if (d_extended_correlation_in_fpga == true) - { - // Now we can write the secondary codes that do not depend on the PRN number - if (trk_parameters.system == 'G') - { - if (signal_type == "L5") - { - if (trk_parameters.track_pilot) - { - multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length, d_data_secondary_code_length); - multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string); - multicorrelator_fpga->initialize_secondary_code(1, d_data_secondary_code_string); - } - else - { - multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length, 0); - multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string); - } - } - } - else if (trk_parameters.system == 'E') - { - if (signal_type == "5X") - { - // coherent integration in the FPGA is only enabled when tracking the pilot. - if (trk_parameters.track_pilot) - { - multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length, d_data_secondary_code_length); - multicorrelator_fpga->initialize_secondary_code(1, d_data_secondary_code_string); - } - - } - } - } - } + if (d_enable_extended_integration == true) + { + if (d_extended_correlation_in_fpga == true) + { + // Now we can write the secondary codes that do not depend on the PRN number + if (trk_parameters.system == 'G') + { + if (signal_type == "L5") + { + if (trk_parameters.track_pilot) + { + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length, d_data_secondary_code_length); + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string); + multicorrelator_fpga->initialize_secondary_code(1, d_data_secondary_code_string); + } + else + { + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length, 0); + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string); + } + } + } + else if (trk_parameters.system == 'E') + { + if (signal_type == "5X") + { + // coherent integration in the FPGA is only enabled when tracking the pilot. + if (trk_parameters.track_pilot) + { + multicorrelator_fpga->set_secondary_code_lengths(d_secondary_code_length, d_data_secondary_code_length); + multicorrelator_fpga->initialize_secondary_code(1, d_data_secondary_code_string); + } + } + } + } + } } @@ -1373,9 +1366,9 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_acquisition_gnss_synchro = p_gnss_synchro; if (p_gnss_synchro->PRN > 0) { - // A set_gnss_synchro command with a valid PRN is received when the system is going to run acquisition with that PRN. + // A set_gnss_synchro command with a valid PRN is received when the system is going to run acquisition with that PRN. // We can use this command to pre-initialize tracking parameters and variables before the actual acquisition process takes place. - // In this way we minimize the latency between acquisition and tracking once the acquisition has been made. + // In this way we minimize the latency between acquisition and tracking once the acquisition has been made. d_sample_counter = 0; d_sample_counter_next = 0; @@ -1390,7 +1383,6 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) { d_Prompt_Data[0] = gr_complex(0.0, 0.0); } - } else if (systemName == "Galileo" and signal_type == "1B") { @@ -1403,19 +1395,17 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) { if (trk_parameters.track_pilot) { - - d_secondary_code_string = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); d_Prompt_Data[0] = gr_complex(0.0, 0.0); - if (d_enable_extended_integration == true) - { - if (d_extended_correlation_in_fpga == true) - { - multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string); - } - } + if (d_enable_extended_integration == true) + { + if (d_extended_correlation_in_fpga == true) + { + multicorrelator_fpga->initialize_secondary_code(0, d_secondary_code_string); + } + } } } @@ -1460,27 +1450,24 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_Prompt_circular_buffer.clear(); - T_chip_seconds = 1.0 / d_code_freq_chips; - T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); + T_chip_seconds = 1.0 / d_code_freq_chips; + T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); - // re-establish nominal integration length (not extended integration by default) - d_current_integration_length_samples = static_cast(trk_parameters.vector_length); - d_next_integration_length_samples = d_current_integration_length_samples; + // re-establish nominal integration length (not extended integration by default) + d_current_integration_length_samples = static_cast(trk_parameters.vector_length); + d_next_integration_length_samples = d_current_integration_length_samples; - multicorrelator_fpga->disable_secondary_codes(); // make sure the processing of the secondary codes is disabled by default + multicorrelator_fpga->disable_secondary_codes(); // make sure the processing of the secondary codes is disabled by default - d_current_fpga_integration_period = 1; - d_current_extended_correlation_in_fpga = false; + d_current_fpga_integration_period = 1; + d_current_extended_correlation_in_fpga = false; } - - - } void dll_pll_veml_tracking_fpga::stop_tracking() { - d_stop_tracking = true; + d_stop_tracking = true; } @@ -1499,506 +1486,494 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un Gnss_Synchro current_synchro_data = Gnss_Synchro(); current_synchro_data.Flag_valid_symbol_output = false; - while((!current_synchro_data.Flag_valid_symbol_output) && (!d_stop_tracking)) - { - - d_current_integration_length_samples = d_next_integration_length_samples; - - if (d_pull_in_transitory == true) - { - if (d_sample_counter > 0) // do not execute this condition until the sample counter has ben read for the first time after start_tracking - { - if (trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) - { - d_pull_in_transitory = false; - d_carrier_lock_fail_counter = 0; - d_code_lock_fail_counter = 0; - } - } - } - switch (d_state) - { - case 1: // Pull-in - { - - d_worker_is_done = false; - - - boost::mutex::scoped_lock lock(d_mutex); - while (!d_worker_is_done) m_condition.wait(lock); - - // Signal alignment (skip samples until the incoming signal is aligned with local replica) - - int64_t acq_trk_diff_samples; - double acq_trk_diff_seconds; - double delta_trk_to_acq_prn_start_samples; - - multicorrelator_fpga->lock_channel(); - uint64_t counter_value = multicorrelator_fpga->read_sample_counter(); - uint64_t absolute_samples_offset; - if (counter_value > (d_acq_sample_stamp + d_acq_code_phase_samples)) - { - // Signal alignment (skip samples until the incoming signal is aligned with local replica) - acq_trk_diff_samples = static_cast(counter_value) - static_cast(d_acq_sample_stamp); - acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; - delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) - d_acq_code_phase_samples; - - uint32_t num_frames = ceil((delta_trk_to_acq_prn_start_samples) / d_current_integration_length_samples); - absolute_samples_offset = static_cast(d_acq_code_phase_samples + d_acq_sample_stamp + num_frames * d_current_integration_length_samples); - } - else - { - // test mode - - acq_trk_diff_samples = -static_cast(counter_value) + static_cast(d_acq_sample_stamp); - acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; - delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) + d_acq_code_phase_samples; - - absolute_samples_offset = static_cast(delta_trk_to_acq_prn_start_samples); - } - - - multicorrelator_fpga->set_initial_sample(absolute_samples_offset); - //d_absolute_samples_offset = absolute_samples_offset; - d_sample_counter = absolute_samples_offset; - current_synchro_data.Tracking_sample_counter = absolute_samples_offset; - d_sample_counter_next = d_sample_counter; - - // Doppler effect Fd = (C / (C + Vr)) * F - double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; - // new chip and PRN sequence periods based on acq Doppler - d_code_freq_chips = radial_velocity * d_code_chip_rate; - d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; - d_code_phase_rate_step_chips = 0.0; - - d_acq_code_phase_samples = absolute_samples_offset; - - d_current_integration_length_samples = trk_parameters.vector_length; - - d_next_integration_length_samples = d_current_integration_length_samples; - - int32_t samples_offset = round(d_acq_code_phase_samples); - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); - d_state = 2; - d_cn0_smoother.reset(); - d_carrier_lock_test_smoother.reset(); - - LOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; - DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz - << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; - - - // // DEBUG OUTPUT - // std::cout << "Tracking of " << systemName << " " << signal_pretty_name << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - // DLOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - - // DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; - // std::cout << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)" << std::endl; - // DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz - // << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; - - break; - } - case 2: // Wide tracking and symbol synchronization - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - - do_correlation_step(); - - // Save single correlation step variables - - if (d_veml) - { - d_VE_accu = *d_Very_Early; - d_VL_accu = *d_Very_Late; - } - d_E_accu = *d_Early; - d_P_accu = *d_Prompt; - d_L_accu = *d_Late; - - //fail-safe: check if the secondary code or bit synchronization has not succedded 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)) - { - d_carrier_lock_fail_counter = 300000; //force loss-of-lock condition - LOG(INFO) << systemName << " " << signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - } - - // Check lock status - - if (!cn0_and_tracking_lock_status(d_code_period)) - { - clear_tracking_vars(); - d_state = 1; // loss-of-lock detected - - // send something to let the scheduler know that it has to keep on calling general work and to finish the loop - //current_synchro_data.Flag_valid_symbol_output=1; - } - else - { - bool next_state = false; - - // Perform DLL/PLL tracking loop computations. Costas Loop enabled - run_dll_pll(); - - update_tracking_vars(); - - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); - - if (!d_pull_in_transitory) - { - if (d_secondary) - { - // ####### SECONDARY CODE LOCK ##### - d_Prompt_circular_buffer.push_back(*d_Prompt); - - if (d_Prompt_circular_buffer.size() == d_secondary_code_length) - { - next_state = acquire_secondary(); - - if (next_state) - { - LOG(INFO) << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - } - } - } - else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change - { - - //******* preamble correlation ******** - d_Prompt_circular_buffer.push_back(*d_Prompt); - if (d_Prompt_circular_buffer.size() == d_secondary_code_length) - { - next_state = acquire_secondary(); - if (next_state) - { - LOG(INFO) << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - std::cout << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - } - } - - } - else - { - next_state = true; - } - } - else - { - next_state = false; //keep in state 2 during pull-in transitory - } - - if (next_state) - { // reset extended correlator - d_VE_accu = gr_complex(0.0, 0.0); - d_E_accu = gr_complex(0.0, 0.0); - d_P_accu = gr_complex(0.0, 0.0); - d_P_data_accu = gr_complex(0.0, 0.0); - d_L_accu = gr_complex(0.0, 0.0); - d_VL_accu = gr_complex(0.0, 0.0); - d_Prompt_circular_buffer.clear(); - d_current_symbol = 0; - d_current_data_symbol = 0; - - if (d_enable_extended_integration) - { - // update integration time - d_extend_correlation_symbols_count = 0; - d_current_correlation_time_s = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); - - if (d_extended_correlation_in_fpga) - { - - d_current_fpga_integration_period = d_fpga_integration_period; - d_current_extended_correlation_in_fpga = true; - - d_P_accu_old.real(d_P_accu_old.real()*d_fpga_integration_period); - d_P_accu_old.imag(d_P_accu_old.imag()*d_fpga_integration_period); - - if (d_sc_demodulate_enabled) - { - multicorrelator_fpga->enable_secondary_codes(); - } - - if (d_extend_fpga_integration_periods > 1) - { - // correction on already computed parameters - K_blk_samples = T_prn_samples*(d_fpga_integration_period) + d_rem_code_phase_samples_prev; - d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); - - d_state = 5; - } - else - { - // correction on already computed parameters - K_blk_samples = T_prn_samples*trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples_prev; - d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); - - d_state = 6; - } - } - else - { - d_state = 3; // next state is the extended correlator integrator - } - - LOG(INFO) << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " - << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); - std::cout << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " - << d_channel - << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - // Set narrow taps delay values [chips] - d_code_loop_filter.set_update_interval(d_current_correlation_time_s); - d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz); - d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order); - if (d_veml) - { - d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); - 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); - } - 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); - } - } - else - { - d_state = 4; - } - } - } - - break; - } - case 3: // coherent integration (correlation time extension) - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - - // perform a correlation step - do_correlation_step(); - save_correlation_results(); - update_tracking_vars(); - - if (d_current_data_symbol == 0) - { - log_data(); - // ########### Output the tracking results to Telemetry block ########## - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); - current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - current_synchro_data.Flag_valid_symbol_output = true; - d_P_data_accu = gr_complex(0.0, 0.0); - } - - d_extend_correlation_symbols_count++; - if (d_extend_correlation_symbols_count == (trk_parameters.extend_correlation_symbols - 1)) - { - d_extend_correlation_symbols_count = 0; - d_state = 4; - } - break; - } - case 4: // narrow tracking - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - - // perform a correlation step - do_correlation_step(); - save_correlation_results(); - - // check lock status - if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) - { - clear_tracking_vars(); - d_state = 1; // loss-of-lock detected - - // send something to let the scheduler know that it has to keep on calling general work and to finish the loop - //current_synchro_data.Flag_valid_symbol_output=1; - - } - else - { - run_dll_pll(); - update_tracking_vars(); - - if (d_current_data_symbol == 0) - { - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); - // ########### Output the tracking results to Telemetry block ########## - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); - current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - current_synchro_data.Flag_valid_symbol_output = true; - d_P_data_accu = gr_complex(0.0, 0.0); - } - - // reset extended correlator - d_VE_accu = gr_complex(0.0, 0.0); - d_E_accu = gr_complex(0.0, 0.0); - d_P_accu = gr_complex(0.0, 0.0); - d_L_accu = gr_complex(0.0, 0.0); - d_VL_accu = gr_complex(0.0, 0.0); - if (d_enable_extended_integration) - { - d_state = 3; // new coherent integration (correlation time extension) cycle - } - } - break; - } - - case 5: // coherent integration (correlation time extension) - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - - // this must be computed for the secondary prn code - if (d_secondary) - { - - uint32_t first_length_secondary_code = d_current_integration_length_samples - (d_fpga_integration_period - 1)*static_cast(std::floor(T_prn_samples)); - uint32_t next_length_secondary_code = static_cast(std::floor(T_prn_samples)); - - multicorrelator_fpga->update_secondary_code_length(first_length_secondary_code, next_length_secondary_code); - } - - // perform a correlation step - do_correlation_step(); - save_correlation_results(); - update_tracking_vars(); - - if (d_current_data_symbol == 0) - { - log_data(); - // ########### Output the tracking results to Telemetry block ########## - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); - current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - current_synchro_data.Flag_valid_symbol_output = true; - d_P_data_accu = gr_complex(0.0, 0.0); - } - - d_extend_correlation_symbols_count++; - if (d_extend_correlation_symbols_count == (d_extend_fpga_integration_periods - 1)) - { - d_extend_correlation_symbols_count = 0; - d_state = 6; - } - - break; - } - - - case 6: // narrow tracking IN THE FPGA - { - d_sample_counter = d_sample_counter_next; - d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); - - // this must be computed for the secondary prn code - if (d_secondary) - { - uint32_t first_length_secondary_code = d_current_integration_length_samples - (d_fpga_integration_period - 1)*static_cast(std::floor(T_prn_samples)); - uint32_t next_length_secondary_code = static_cast(std::floor(T_prn_samples)); - - multicorrelator_fpga->update_secondary_code_length(first_length_secondary_code, next_length_secondary_code); - } - - // perform a correlation step - do_correlation_step(); - save_correlation_results(); - // check lock status - if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) - { - clear_tracking_vars(); - d_state = 1; // loss-of-lock detected - - // send something to let the scheduler know that it has to keep on calling general work and to finish the loop - //current_synchro_data.Flag_valid_symbol_output=1; - - } - else - { - run_dll_pll(); - update_tracking_vars(); - - if (d_current_data_symbol == 0) - { - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); - // ########### Output the tracking results to Telemetry block ########## - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); - current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - current_synchro_data.Flag_valid_symbol_output = true; - d_P_data_accu = gr_complex(0.0, 0.0); - } - - d_extend_correlation_symbols_count = 0; - - // reset extended correlator - d_VE_accu = gr_complex(0.0, 0.0); - d_E_accu = gr_complex(0.0, 0.0); - d_P_accu = gr_complex(0.0, 0.0); - d_L_accu = gr_complex(0.0, 0.0); - d_VL_accu = gr_complex(0.0, 0.0); - - if (d_extend_fpga_integration_periods > 1) - { - d_state = 5; - } - - - } - break; - } - } - } + while ((!current_synchro_data.Flag_valid_symbol_output) && (!d_stop_tracking)) + { + d_current_integration_length_samples = d_next_integration_length_samples; + + if (d_pull_in_transitory == true) + { + if (d_sample_counter > 0) // do not execute this condition until the sample counter has ben read for the first time after start_tracking + { + if (trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) + { + d_pull_in_transitory = false; + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + } + } + } + switch (d_state) + { + case 1: // Pull-in + { + d_worker_is_done = false; + + + boost::mutex::scoped_lock lock(d_mutex); + while (!d_worker_is_done) m_condition.wait(lock); + + // Signal alignment (skip samples until the incoming signal is aligned with local replica) + + int64_t acq_trk_diff_samples; + double acq_trk_diff_seconds; + double delta_trk_to_acq_prn_start_samples; + + multicorrelator_fpga->lock_channel(); + uint64_t counter_value = multicorrelator_fpga->read_sample_counter(); + uint64_t absolute_samples_offset; + if (counter_value > (d_acq_sample_stamp + d_acq_code_phase_samples)) + { + // Signal alignment (skip samples until the incoming signal is aligned with local replica) + acq_trk_diff_samples = static_cast(counter_value) - static_cast(d_acq_sample_stamp); + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; + delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) - d_acq_code_phase_samples; + + uint32_t num_frames = ceil((delta_trk_to_acq_prn_start_samples) / d_current_integration_length_samples); + absolute_samples_offset = static_cast(d_acq_code_phase_samples + d_acq_sample_stamp + num_frames * d_current_integration_length_samples); + } + else + { + // test mode + + acq_trk_diff_samples = -static_cast(counter_value) + static_cast(d_acq_sample_stamp); + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / trk_parameters.fs_in; + delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) + d_acq_code_phase_samples; + + absolute_samples_offset = static_cast(delta_trk_to_acq_prn_start_samples); + } + + + multicorrelator_fpga->set_initial_sample(absolute_samples_offset); + //d_absolute_samples_offset = absolute_samples_offset; + d_sample_counter = absolute_samples_offset; + current_synchro_data.Tracking_sample_counter = absolute_samples_offset; + d_sample_counter_next = d_sample_counter; + + // Doppler effect Fd = (C / (C + Vr)) * F + double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; + // new chip and PRN sequence periods based on acq Doppler + d_code_freq_chips = radial_velocity * d_code_chip_rate; + d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; + d_code_phase_rate_step_chips = 0.0; + + d_acq_code_phase_samples = absolute_samples_offset; + + d_current_integration_length_samples = trk_parameters.vector_length; + + d_next_integration_length_samples = d_current_integration_length_samples; + + int32_t samples_offset = round(d_acq_code_phase_samples); + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); + d_state = 2; + d_cn0_smoother.reset(); + d_carrier_lock_test_smoother.reset(); + + LOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; + DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz + << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; + + + // // DEBUG OUTPUT + // std::cout << "Tracking of " << systemName << " " << signal_pretty_name << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + // DLOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + + // DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; + // std::cout << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)" << std::endl; + // DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz + // << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; + + break; + } + case 2: // Wide tracking and symbol synchronization + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + do_correlation_step(); + + // Save single correlation step variables + + if (d_veml) + { + d_VE_accu = *d_Very_Early; + d_VL_accu = *d_Very_Late; + } + d_E_accu = *d_Early; + d_P_accu = *d_Prompt; + d_L_accu = *d_Late; + + //fail-safe: check if the secondary code or bit synchronization has not succedded 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)) + { + d_carrier_lock_fail_counter = 300000; //force loss-of-lock condition + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + + // Check lock status + + if (!cn0_and_tracking_lock_status(d_code_period)) + { + clear_tracking_vars(); + d_state = 1; // loss-of-lock detected + + // send something to let the scheduler know that it has to keep on calling general work and to finish the loop + //current_synchro_data.Flag_valid_symbol_output=1; + } + else + { + bool next_state = false; + + // Perform DLL/PLL tracking loop computations. Costas Loop enabled + run_dll_pll(); + + update_tracking_vars(); + + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + + if (!d_pull_in_transitory) + { + if (d_secondary) + { + // ####### SECONDARY CODE LOCK ##### + d_Prompt_circular_buffer.push_back(*d_Prompt); + + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); + + if (next_state) + { + LOG(INFO) << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + } + } + else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change + { + //******* preamble correlation ******** + d_Prompt_circular_buffer.push_back(*d_Prompt); + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); + if (next_state) + { + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + std::cout << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + } + } + else + { + next_state = true; + } + } + else + { + next_state = false; //keep in state 2 during pull-in transitory + } + + if (next_state) + { // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + d_Prompt_circular_buffer.clear(); + d_current_symbol = 0; + d_current_data_symbol = 0; + + if (d_enable_extended_integration) + { + // update integration time + d_extend_correlation_symbols_count = 0; + d_current_correlation_time_s = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); + + if (d_extended_correlation_in_fpga) + { + d_current_fpga_integration_period = d_fpga_integration_period; + d_current_extended_correlation_in_fpga = true; + + d_P_accu_old.real(d_P_accu_old.real() * d_fpga_integration_period); + d_P_accu_old.imag(d_P_accu_old.imag() * d_fpga_integration_period); + + if (d_sc_demodulate_enabled) + { + multicorrelator_fpga->enable_secondary_codes(); + } + + if (d_extend_fpga_integration_periods > 1) + { + // correction on already computed parameters + K_blk_samples = T_prn_samples * (d_fpga_integration_period) + d_rem_code_phase_samples_prev; + d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); + + d_state = 5; + } + else + { + // correction on already computed parameters + K_blk_samples = T_prn_samples * trk_parameters.extend_correlation_symbols + d_rem_code_phase_samples_prev; + d_next_integration_length_samples = static_cast(std::floor(K_blk_samples)); + + d_state = 6; + } + } + else + { + d_state = 3; // next state is the extended correlator integrator + } + + LOG(INFO) << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " + << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + std::cout << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " + << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + // Set narrow taps delay values [chips] + d_code_loop_filter.set_update_interval(d_current_correlation_time_s); + d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz); + d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order); + if (d_veml) + { + d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + 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); + } + 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); + } + } + else + { + d_state = 4; + } + } + } + + break; + } + case 3: // coherent integration (correlation time extension) + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + // perform a correlation step + do_correlation_step(); + save_correlation_results(); + update_tracking_vars(); + + if (d_current_data_symbol == 0) + { + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count == (trk_parameters.extend_correlation_symbols - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 4; + } + break; + } + case 4: // narrow tracking + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + // perform a correlation step + do_correlation_step(); + save_correlation_results(); + + // check lock status + if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) + { + clear_tracking_vars(); + d_state = 1; // loss-of-lock detected + + // send something to let the scheduler know that it has to keep on calling general work and to finish the loop + //current_synchro_data.Flag_valid_symbol_output=1; + } + else + { + run_dll_pll(); + update_tracking_vars(); + + if (d_current_data_symbol == 0) + { + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + + // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + if (d_enable_extended_integration) + { + d_state = 3; // new coherent integration (correlation time extension) cycle + } + } + break; + } + + case 5: // coherent integration (correlation time extension) + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + // this must be computed for the secondary prn code + if (d_secondary) + { + uint32_t first_length_secondary_code = d_current_integration_length_samples - (d_fpga_integration_period - 1) * static_cast(std::floor(T_prn_samples)); + uint32_t next_length_secondary_code = static_cast(std::floor(T_prn_samples)); + + multicorrelator_fpga->update_secondary_code_length(first_length_secondary_code, next_length_secondary_code); + } + + // perform a correlation step + do_correlation_step(); + save_correlation_results(); + update_tracking_vars(); + + if (d_current_data_symbol == 0) + { + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count == (d_extend_fpga_integration_periods - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 6; + } + + break; + } + + + case 6: // narrow tracking IN THE FPGA + { + d_sample_counter = d_sample_counter_next; + d_sample_counter_next = d_sample_counter + static_cast(d_current_integration_length_samples); + + // this must be computed for the secondary prn code + if (d_secondary) + { + uint32_t first_length_secondary_code = d_current_integration_length_samples - (d_fpga_integration_period - 1) * static_cast(std::floor(T_prn_samples)); + uint32_t next_length_secondary_code = static_cast(std::floor(T_prn_samples)); + + multicorrelator_fpga->update_secondary_code_length(first_length_secondary_code, next_length_secondary_code); + } + + // perform a correlation step + do_correlation_step(); + save_correlation_results(); + // check lock status + if (!cn0_and_tracking_lock_status(d_code_period * static_cast(trk_parameters.extend_correlation_symbols))) + { + clear_tracking_vars(); + d_state = 1; // loss-of-lock detected + + // send something to let the scheduler know that it has to keep on calling general work and to finish the loop + //current_synchro_data.Flag_valid_symbol_output=1; + } + else + { + run_dll_pll(); + update_tracking_vars(); + + if (d_current_data_symbol == 0) + { + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + + d_extend_correlation_symbols_count = 0; + + // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + + if (d_extend_fpga_integration_periods > 1) + { + d_state = 5; + } + } + break; + } + } + } if (current_synchro_data.Flag_valid_symbol_output) { current_synchro_data.fs = static_cast(trk_parameters.fs_in); - current_synchro_data.Tracking_sample_counter = d_sample_counter_next; //d_sample_counter; + current_synchro_data.Tracking_sample_counter = d_sample_counter_next; //d_sample_counter; *out[0] = current_synchro_data; return 1; } return 0; - - } diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 3c3e2a203..ff55c5a30 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -153,7 +153,7 @@ private: double d_carrier_phase_step_rad; double d_carrier_phase_rate_step_rad; boost::circular_buffer> d_carr_ph_history; - + // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; double d_rem_code_phase_samples_prev; @@ -221,7 +221,6 @@ private: boost::mutex d_mutex; bool d_stop_tracking; - }; #endif //GNSS_SDR_DLL_PLL_VEML_TRACKING_FPGA_H diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.cc b/src/algorithms/tracking/libs/fpga_multicorrelator.cc index 046bd0050..d3bdd114f 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.cc +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.cc @@ -211,20 +211,20 @@ void Fpga_Multicorrelator_8sc::Carrier_wipeoff_multicorrelator_resampler( // release secondary code indices, keep channel locked if (d_secondary_code_enabled == true) - { - //printf("in the right place\n"); - // debug - force reset counter every time - //d_map_base[DROP_SAMPLES_REG_ADDR] = ENABLE_SECONDARY_CODE | INIT_SECONDARY_CODE_ADDRESSES; - d_map_base[DROP_SAMPLES_REG_ADDR] = ENABLE_SECONDARY_CODE; // keep secondary code enabled + { + //printf("in the right place\n"); + // debug - force reset counter every time + //d_map_base[DROP_SAMPLES_REG_ADDR] = ENABLE_SECONDARY_CODE | INIT_SECONDARY_CODE_ADDRESSES; + d_map_base[DROP_SAMPLES_REG_ADDR] = ENABLE_SECONDARY_CODE; // keep secondary code enabled -// //printf("do not enable secondary code on purpose\n"); -// d_map_base[DROP_SAMPLES_REG_ADDR] = 0; // block samples - } + // //printf("do not enable secondary code on purpose\n"); + // d_map_base[DROP_SAMPLES_REG_ADDR] = 0; // block samples + } else - { - //printf("in the wrong place\n"); - d_map_base[DROP_SAMPLES_REG_ADDR] = 0; // block samples - } + { + //printf("in the wrong place\n"); + d_map_base[DROP_SAMPLES_REG_ADDR] = 0; // block samples + } Fpga_Multicorrelator_8sc::read_tracking_gps_results(); } @@ -476,10 +476,10 @@ void Fpga_Multicorrelator_8sc::read_tracking_gps_results(void) void Fpga_Multicorrelator_8sc::unlock_channel(void) { // unlock the channel to let the next samples go through - d_map_base[DROP_SAMPLES_REG_ADDR] = DROP_SAMPLES; // unlock the channel and disable secondary codes - d_map_base[STOP_TRACKING_REG_ADDR] = 1; // set the tracking module back to idle + d_map_base[DROP_SAMPLES_REG_ADDR] = DROP_SAMPLES; // unlock the channel and disable secondary codes + d_map_base[STOP_TRACKING_REG_ADDR] = 1; // set the tracking module back to idle - d_secondary_code_enabled = false; + d_secondary_code_enabled = false; } @@ -503,55 +503,44 @@ void Fpga_Multicorrelator_8sc::lock_channel(void) void Fpga_Multicorrelator_8sc::set_secondary_code_lengths(uint32_t secondary_code_0_length, uint32_t secondary_code_1_length) { + d_secondary_code_0_length = secondary_code_0_length; + d_secondary_code_1_length = secondary_code_1_length; + // debug + //printf("warning extending the code length 0 to 20\n"); + //d_secondary_code_0_length = 20; + uint32_t secondary_code_length_0_minus_1 = d_secondary_code_0_length - 1; + uint32_t secondary_code_length_1_minus_1 = d_secondary_code_1_length - 1; + d_map_base[SECONDARY_CODE_LENGTHS_REG_ADDR] = secondary_code_length_1_minus_1 * 256 + secondary_code_length_0_minus_1; - - d_secondary_code_0_length = secondary_code_0_length; - d_secondary_code_1_length = secondary_code_1_length; - - // debug - //printf("warning extending the code length 0 to 20\n"); - //d_secondary_code_0_length = 20; - - uint32_t secondary_code_length_0_minus_1 = d_secondary_code_0_length - 1; - uint32_t secondary_code_length_1_minus_1 = d_secondary_code_1_length - 1; - - d_map_base[SECONDARY_CODE_LENGTHS_REG_ADDR] = secondary_code_length_1_minus_1*256 + secondary_code_length_0_minus_1; - - //std::cout << "setting secondary code lengths : \n"; - //std::cout << "initialized correlator 1 sec code length = " << d_secondary_code_1_length << " correlator 0 sec code length = " << d_secondary_code_0_length << std::endl; + //std::cout << "setting secondary code lengths : \n"; + //std::cout << "initialized correlator 1 sec code length = " << d_secondary_code_1_length << " correlator 0 sec code length = " << d_secondary_code_0_length << std::endl; } void Fpga_Multicorrelator_8sc::update_secondary_code_length(uint32_t first_length_secondary_code, uint32_t next_length_secondary_code) { - d_map_base[FIRST_PRN_LENGTH_MINUS_1_REG_ADDR] = first_length_secondary_code - 1; - d_map_base[NEXT_PRN_LENGTH_MINUS_1_REG_ADDR] = next_length_secondary_code - 1; - //std::cout << " first_length_secondary_code = " << first_length_secondary_code << " next_length_secondary_code = " << next_length_secondary_code << " sum = " << first_length_secondary_code + next_length_secondary_code << std::endl; - - - - + d_map_base[FIRST_PRN_LENGTH_MINUS_1_REG_ADDR] = first_length_secondary_code - 1; + d_map_base[NEXT_PRN_LENGTH_MINUS_1_REG_ADDR] = next_length_secondary_code - 1; + //std::cout << " first_length_secondary_code = " << first_length_secondary_code << " next_length_secondary_code = " << next_length_secondary_code << " sum = " << first_length_secondary_code + next_length_secondary_code << std::endl; } void Fpga_Multicorrelator_8sc::initialize_secondary_code(uint32_t secondary_code, std::string *secondary_code_string) { - uint32_t secondary_code_length; - uint32_t reg_addr; - if (secondary_code == 0) - { - secondary_code_length = d_secondary_code_0_length; - reg_addr = PROG_SECONDARY_CODE_0_DATA_REG_ADDR; - - } - else - { - secondary_code_length = d_secondary_code_1_length; - reg_addr = PROG_SECONDARY_CODE_1_DATA_REG_ADDR; - } - Fpga_Multicorrelator_8sc::write_secondary_code(secondary_code_length, secondary_code_string, reg_addr); - + uint32_t secondary_code_length; + uint32_t reg_addr; + if (secondary_code == 0) + { + secondary_code_length = d_secondary_code_0_length; + reg_addr = PROG_SECONDARY_CODE_0_DATA_REG_ADDR; + } + else + { + secondary_code_length = d_secondary_code_1_length; + reg_addr = PROG_SECONDARY_CODE_1_DATA_REG_ADDR; + } + Fpga_Multicorrelator_8sc::write_secondary_code(secondary_code_length, secondary_code_string, reg_addr); } @@ -587,140 +576,139 @@ void Fpga_Multicorrelator_8sc::initialize_secondary_code(uint32_t secondary_code void Fpga_Multicorrelator_8sc::write_secondary_code(uint32_t secondary_code_length, std::string *secondary_code_string, uint32_t reg_addr) { - uint32_t num_words = ceil(((float) secondary_code_length)/SECONDARY_CODE_WORD_SIZE); - uint32_t last_word_size = secondary_code_length % SECONDARY_CODE_WORD_SIZE; - //uint32_t initial_pointer; + uint32_t num_words = ceil(((float)secondary_code_length) / SECONDARY_CODE_WORD_SIZE); + uint32_t last_word_size = secondary_code_length % SECONDARY_CODE_WORD_SIZE; + //uint32_t initial_pointer; - if (last_word_size == 0) - { - last_word_size = SECONDARY_CODE_WORD_SIZE; - } - // debug - //std::cout << "secondary_code_length = " << secondary_code_length << std::endl; - //std::cout << "secondary code string = " << *secondary_code_string << std::endl; - //std::cout << "reg_addr = " << reg_addr << std::endl; + if (last_word_size == 0) + { + last_word_size = SECONDARY_CODE_WORD_SIZE; + } + // debug + //std::cout << "secondary_code_length = " << secondary_code_length << std::endl; + //std::cout << "secondary code string = " << *secondary_code_string << std::endl; + //std::cout << "reg_addr = " << reg_addr << std::endl; - // debug - //std::cout << "num_words = " << num_words << std::endl; - //std::cout << "last_word_size = " << last_word_size << std::endl; + // debug + //std::cout << "num_words = " << num_words << std::endl; + //std::cout << "last_word_size = " << last_word_size << std::endl; - uint32_t write_val = 0U; - uint32_t pow_k; - uint32_t mem_addr; - if (num_words > 1) - { - for (mem_addr = 0; mem_addr < num_words - 1 ;mem_addr++) - { - //std::cout << "------------------------------------------------------ going to write word " << mem_addr << std::endl; - write_val = 0U; - pow_k = 1; - for (unsigned int k=0;k>= 1; -// } -// printf("\n"); - } - } - write_val = 0U; - pow_k = 1; - mem_addr = num_words - 1; + // debug + // std::cout << "wrote word " << mem_addr << "value is "; + // while (write_val) { + // if (write_val & 1) + // printf("1"); + // else + // printf("0"); + // + // write_val >>= 1; + // } + // printf("\n"); + } + } + write_val = 0U; + pow_k = 1; + mem_addr = num_words - 1; - //std::cout << "------------------------------------------------------ going to write word " << mem_addr << std::endl; + //std::cout << "------------------------------------------------------ going to write word " << mem_addr << std::endl; - for (unsigned int k=0;k>= 1; -// } -// printf("\n"); - //printf("\n=============================================================================* END OF THIS\n"); + // // debug + // std::cout << "wrote word " << mem_addr << " value is " << write_val << " = "; + // while (write_val) { + // if (write_val & 1) + // printf("1"); + // else + // printf("0"); + // + // write_val >>= 1; + // } + // printf("\n"); + //printf("\n=============================================================================* END OF THIS\n"); } //void Fpga_Multicorrelator_8sc::init_secondary_code_indices(void) @@ -730,19 +718,18 @@ void Fpga_Multicorrelator_8sc::write_secondary_code(uint32_t secondary_code_leng void Fpga_Multicorrelator_8sc::enable_secondary_codes() { - d_map_base[DROP_SAMPLES_REG_ADDR] = INIT_SECONDARY_CODE_ADDRESSES | ENABLE_SECONDARY_CODE; // enable secondary codes and clear secondary code indices - d_secondary_code_enabled = true; - //std::cout << "enabling secondary codes d_map_base[DROP_SAMPLES_REG_ADDR] = " << (INIT_SECONDARY_CODE_ADDRESSES | ENABLE_SECONDARY_CODE) << std::endl; + d_map_base[DROP_SAMPLES_REG_ADDR] = INIT_SECONDARY_CODE_ADDRESSES | ENABLE_SECONDARY_CODE; // enable secondary codes and clear secondary code indices + d_secondary_code_enabled = true; + //std::cout << "enabling secondary codes d_map_base[DROP_SAMPLES_REG_ADDR] = " << (INIT_SECONDARY_CODE_ADDRESSES | ENABLE_SECONDARY_CODE) << std::endl; -// // debug -// printf("do not enable secondary code on purpose\n"); -// d_map_base[DROP_SAMPLES_REG_ADDR] = 0; + // // debug + // printf("do not enable secondary code on purpose\n"); + // d_map_base[DROP_SAMPLES_REG_ADDR] = 0; } void Fpga_Multicorrelator_8sc::disable_secondary_codes() { - // this function is to be called before starting the tracking process in order to disable the secondary codes by default - //printf("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx disabling secondary codes in fpga\n"); - d_map_base[DROP_SAMPLES_REG_ADDR] = DROP_SAMPLES; + // this function is to be called before starting the tracking process in order to disable the secondary codes by default + //printf("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx disabling secondary codes in fpga\n"); + d_map_base[DROP_SAMPLES_REG_ADDR] = DROP_SAMPLES; } - diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.h b/src/algorithms/tracking/libs/fpga_multicorrelator.h index 890a39289..1983215fb 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.h +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.h @@ -73,12 +73,12 @@ #define SAMPLE_COUNTER_REG_ADDR_MSW 14 // FPGA-related constants -#define SECONDARY_CODE_WORD_SIZE 20 // the secondary codes are written in to the FPGA in words of SECONDARY_CODE_WORD_SIZE bits -#define SECONDARY_CODE_WR_STROBE 0x800000 // write strobe position in the secondary code write register -#define SECONDARY_CODE_ADDR_BITS 0x100000 // memory address position in the secondary code write register -#define DROP_SAMPLES 1 // bit 0 of DROP_SAMPLES_REG_ADDR -#define ENABLE_SECONDARY_CODE 2 // bit 1 of DROP_SAMPLES_REG_ADDR -#define INIT_SECONDARY_CODE_ADDRESSES 4 // bit 2 of DROP_SAMPLES_REG_ADDR +#define SECONDARY_CODE_WORD_SIZE 20 // the secondary codes are written in to the FPGA in words of SECONDARY_CODE_WORD_SIZE bits +#define SECONDARY_CODE_WR_STROBE 0x800000 // write strobe position in the secondary code write register +#define SECONDARY_CODE_ADDR_BITS 0x100000 // memory address position in the secondary code write register +#define DROP_SAMPLES 1 // bit 0 of DROP_SAMPLES_REG_ADDR +#define ENABLE_SECONDARY_CODE 2 // bit 1 of DROP_SAMPLES_REG_ADDR +#define INIT_SECONDARY_CODE_ADDRESSES 4 // bit 2 of DROP_SAMPLES_REG_ADDR /*! * \brief Class that implements carrier wipe-off and correlators. @@ -105,15 +105,15 @@ public: uint64_t read_sample_counter(); void lock_channel(void); void unlock_channel(void); -// void initialize_secondary_codes(bool track_pilot, -// uint32_t secondary_code_length_data, std::string *secondary_code_string_data, -// uint32_t secondary_code_length_pilot, std::string *secondary_code_string_pilot); + // void initialize_secondary_codes(bool track_pilot, + // uint32_t secondary_code_length_data, std::string *secondary_code_string_data, + // uint32_t secondary_code_length_pilot, std::string *secondary_code_string_pilot); void set_secondary_code_lengths(uint32_t secondary_code_0_length, uint32_t secondary_code_1_length); void initialize_secondary_code(uint32_t secondary_code, std::string *secondary_code_string); void update_secondary_code_length(uint32_t first_length_secondary_code, uint32_t next_length_secondary_code); void enable_secondary_codes(); void disable_secondary_codes(); -// void init_secondary_code_indices(); + // void init_secondary_code_indices(); private: diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc index 747e4cbc5..e0e2f0cb9 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc @@ -209,8 +209,8 @@ GpsL1CADllPllTrackingTestFpga_msg_rx::GpsL1CADllPllTrackingTestFpga_msg_rx() : g this->message_port_register_in(pmt::mp("events")); this->set_msg_handler(pmt::mp("events"), boost::bind( - &GpsL1CADllPllTrackingTestFpga_msg_rx::msg_handler_events, - this, _1)); + &GpsL1CADllPllTrackingTestFpga_msg_rx::msg_handler_events, + this, _1)); rx_message = 0; } From 3225d92f1bdcb642bfed4e99e5575445ff0da337 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Fri, 19 Jul 2019 12:24:25 +0200 Subject: [PATCH 13/22] removed unused variable --- .../acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc index 17d98fc25..f8299e4ba 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc @@ -87,7 +87,6 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( // The FPGA can only use FFT lengths that are a power of two. float nbits = ceilf(log2f((float)code_length)); unsigned int nsamples_total = pow(2, nbits); - unsigned int vector_length = nsamples_total; unsigned int select_queue_Fpga = configuration_->property(role + ".select_queue_Fpga", 0); acq_parameters.select_queue_Fpga = select_queue_Fpga; std::string default_device_name = "/dev/uio0"; From f42629438555e82f1f1ec691930346912971e0a0 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Fri, 19 Jul 2019 13:04:39 +0200 Subject: [PATCH 14/22] removed unnecessary messages --- .../tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc | 3 --- .../tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc | 1 - .../tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc | 3 --- 3 files changed, 7 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index 918750cf1..af395fa21 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -261,7 +261,6 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( { trk_param_fpga.extended_correlation_in_fpga = true; trk_param_fpga.fpga_integration_period = extend_correlation_symbols; - printf("correlation in fpga true\n"); } } else @@ -271,8 +270,6 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( trk_param_fpga.extended_correlation_in_fpga = true; trk_param_fpga.extend_fpga_integration_periods = extend_correlation_symbols / GALILEO_E5A_I_SECONDARY_CODE_LENGTH; trk_param_fpga.fpga_integration_period = GALILEO_E5A_I_SECONDARY_CODE_LENGTH; - printf("correlation in fpga true\n"); - printf("extend fpga integration periods true\n"); } } } diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index e3cc0b964..98e7727f4 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -225,7 +225,6 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( { trk_param_fpga.extended_correlation_in_fpga = true; trk_param_fpga.fpga_integration_period = symbols_extended_correlator; - printf("correlation in fpga true\n"); } } } diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index 73903399f..6b5dc9b94 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -286,7 +286,6 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( { trk_param_fpga.extended_correlation_in_fpga = true; trk_param_fpga.fpga_integration_period = extend_correlation_symbols; - printf("correlation in fpga true\n"); } } else @@ -296,8 +295,6 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( trk_param_fpga.extended_correlation_in_fpga = true; trk_param_fpga.extend_fpga_integration_periods = extend_correlation_symbols / GPS_L5I_NH_CODE_LENGTH; trk_param_fpga.fpga_integration_period = GPS_L5I_NH_CODE_LENGTH; - printf("correlation in fpga true\n"); - printf("extend fpga integration periods true\n"); } } } From 851d36cb9b8f896b8df4d20b21b22f07a7edf197 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Fri, 19 Jul 2019 17:23:00 +0200 Subject: [PATCH 15/22] minor code optimizations + replaced #defines by constants in the tracking adapters --- .../galileo_e1_dll_pll_veml_tracking_fpga.cc | 6 ------ .../galileo_e1_dll_pll_veml_tracking_fpga.h | 6 ++++++ .../galileo_e5a_dll_pll_tracking_fpga.cc | 6 ------ .../galileo_e5a_dll_pll_tracking_fpga.h | 7 +++++++ .../gps_l1_ca_dll_pll_tracking_fpga.cc | 7 ------- .../adapters/gps_l1_ca_dll_pll_tracking_fpga.h | 8 ++++++++ .../adapters/gps_l2_m_dll_pll_tracking_fpga.cc | 5 +---- .../adapters/gps_l2_m_dll_pll_tracking_fpga.h | 1 + .../adapters/gps_l5_dll_pll_tracking_fpga.cc | 6 ------ .../adapters/gps_l5_dll_pll_tracking_fpga.h | 6 ++++++ .../dll_pll_veml_tracking_fpga.cc | 18 ++++++++---------- 11 files changed, 37 insertions(+), 39 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc index 622e09dcf..a6acbc082 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc @@ -45,12 +45,6 @@ #include #include -// the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000 // flag that enables WE (Write Enable) of the local code FPGA -#define LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT 0x20000000 // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) - - GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h index a81cd7d02..28c732837 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h @@ -98,6 +98,12 @@ public: void stop_tracking() override; private: + + // the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const int32_t LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY = 0x0C000000; // flag that enables WE (Write Enable) of the local code FPGA + static const int32_t LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT = 0x20000000; // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) + dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; size_t item_size_; uint32_t channel_; diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index af395fa21..6a5eeded8 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -40,12 +40,6 @@ #include #include -// the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000 // flag that enables WE (Write Enable) of the local code FPGA -#define LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT 0x20000000 // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) - - GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( ConfigurationInterface *configuration, const std::string &role, unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h index 48a029a22..0a489478d 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h @@ -90,6 +90,13 @@ public: void stop_tracking() override; private: + + // the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const int32_t LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY = 0x0C000000; // flag that enables WE (Write Enable) of the local code FPGA + static const int32_t LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT = 0x20000000; // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) + + dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; size_t item_size_; uint32_t channel_; diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index 98e7727f4..cd8b623ef 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -45,13 +45,6 @@ #include #include -#define NUM_PRNs 32 // total number of PRNs -#define GPS_CA_BIT_DURATION_MS 20 -#define GPS_CA_CODE_PERIOD_MS 1 -// the following flag is FPGA-specific and they are using arrange the values of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000 // flag that enables WE (Write Enable) of the local code FPGA - GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h index ce3ce90fd..a77c2d4d0 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h @@ -96,6 +96,14 @@ public: void stop_tracking() override; private: + + static const uint32_t NUM_PRNs = 32; // total number of PRNs + static const int32_t GPS_CA_BIT_DURATION_MS = 20; + // the following flag is FPGA-specific and they are using arrange the values of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const int32_t LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY = 0x0C000000; // flag that enables WE (Write Enable) of the local code FPGA + + dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; size_t item_size_; uint32_t channel_; diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc index f633c84b2..196bd1736 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc @@ -50,9 +50,6 @@ #include // for memcpy #include -#define NUM_PRNs 32 - - GpsL2MDllPllTrackingFpga::GpsL2MDllPllTrackingFpga( ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) @@ -126,7 +123,7 @@ GpsL2MDllPllTrackingFpga::GpsL2MDllPllTrackingFpga( //################# PRE-COMPUTE ALL THE CODES ################# d_ca_codes = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L2_M_CODE_LENGTH_CHIPS * NUM_PRNs) * sizeof(int), volk_gnsssdr_get_alignment())); - for (unsigned int PRN = 1; PRN <= NUM_PRNs; PRN++) + for (uint32_t PRN = 1; PRN <= NUM_PRNs; PRN++) { gps_l2c_m_code_gen_float(gsl::span(ca_codes_f, static_cast(GPS_L2_M_CODE_LENGTH_CHIPS)), PRN); for (unsigned int s = 0; s < 2 * static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); s++) diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.h index 85dcc4707..96223a816 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.h @@ -99,6 +99,7 @@ public: void stop_tracking() override; private: + static const uint32_t NUM_PRNs = 32; dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; unsigned int channel_; std::string role_; diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index 6b5dc9b94..90c7ab19a 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -49,12 +49,6 @@ #define NUM_PRNs 32 // number of PRNS -// the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000 // flag that enables WE (Write Enable) of the local code FPGA -#define LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT 0x20000000 // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) - - GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( ConfigurationInterface *configuration, const std::string &role, unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h index 2805dbcef..80aff551e 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h @@ -96,6 +96,12 @@ public: void stop_tracking() override; private: + + // the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const int32_t LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY = 0x0C000000; // flag that enables WE (Write Enable) of the local code FPGA + static const int32_t LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT = 0x20000000; // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) + dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; size_t item_size_; uint32_t channel_; 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 2a2ec239c..1d139e26e 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 @@ -1421,6 +1421,8 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_carrier_lock_test = 1.0; d_CN0_SNV_dB_Hz = 0.0; + d_code_phase_rate_step_chips = 0.0; + if (d_veml) { d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); @@ -1461,6 +1463,10 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_current_fpga_integration_period = 1; d_current_extended_correlation_in_fpga = false; + + d_cn0_smoother.reset(); + d_carrier_lock_test_smoother.reset(); + } } @@ -1517,10 +1523,10 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un int64_t acq_trk_diff_samples; double acq_trk_diff_seconds; double delta_trk_to_acq_prn_start_samples; + uint64_t absolute_samples_offset; multicorrelator_fpga->lock_channel(); uint64_t counter_value = multicorrelator_fpga->read_sample_counter(); - uint64_t absolute_samples_offset; if (counter_value > (d_acq_sample_stamp + d_acq_code_phase_samples)) { // Signal alignment (skip samples until the incoming signal is aligned with local replica) @@ -1546,7 +1552,6 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un multicorrelator_fpga->set_initial_sample(absolute_samples_offset); //d_absolute_samples_offset = absolute_samples_offset; d_sample_counter = absolute_samples_offset; - current_synchro_data.Tracking_sample_counter = absolute_samples_offset; d_sample_counter_next = d_sample_counter; // Doppler effect Fd = (C / (C + Vr)) * F @@ -1554,25 +1559,18 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // new chip and PRN sequence periods based on acq Doppler d_code_freq_chips = radial_velocity * d_code_chip_rate; d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; - d_code_phase_rate_step_chips = 0.0; d_acq_code_phase_samples = absolute_samples_offset; - d_current_integration_length_samples = trk_parameters.vector_length; - - d_next_integration_length_samples = d_current_integration_length_samples; - int32_t samples_offset = round(d_acq_code_phase_samples); d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); + d_state = 2; - d_cn0_smoother.reset(); - d_carrier_lock_test_smoother.reset(); LOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; - // // DEBUG OUTPUT // std::cout << "Tracking of " << systemName << " " << signal_pretty_name << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; // DLOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; From 8d813f05d1a5bc0bb81673f1b5df0f893d99617c Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Mon, 22 Jul 2019 11:47:33 +0200 Subject: [PATCH 16/22] updated the calculation of the FPGA integration length variable --- .../tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 1d139e26e..b4b9770d4 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 @@ -806,8 +806,8 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars() K_blk_samples = T_prn_samples * d_current_fpga_integration_period + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function int32_t actual_blk_length = static_cast(std::floor(K_blk_samples)); - d_next_integration_length_samples = 2 * actual_blk_length - d_current_integration_length_samples; - + //d_next_integration_length_samples = 2 * actual_blk_length - d_current_integration_length_samples; + d_next_integration_length_samples = actual_blk_length; //################### PLL COMMANDS ################################################# // carrier phase step (NCO phase increment per sample) [rads/sample] d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in; From 00993d9719955031b3e59fe97ba0ee8555541ab9 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Mon, 22 Jul 2019 19:58:37 +0200 Subject: [PATCH 17/22] replaced #defines by static consts + removed unused tracking parameter + removed unnecessary comments --- ...ileo_e1_pcps_ambiguous_acquisition_fpga.cc | 16 +- ...lileo_e1_pcps_ambiguous_acquisition_fpga.h | 9 + .../galileo_e5a_pcps_acquisition_fpga.cc | 16 +- .../galileo_e5a_pcps_acquisition_fpga.h | 9 + .../gps_l1_ca_pcps_acquisition_fpga.cc | 16 +- .../gps_l1_ca_pcps_acquisition_fpga.h | 10 + .../adapters/gps_l5i_pcps_acquisition_fpga.cc | 17 +- .../adapters/gps_l5i_pcps_acquisition_fpga.h | 9 + .../galileo_e1_dll_pll_veml_tracking_fpga.cc | 9 - .../galileo_e5a_dll_pll_tracking_fpga.cc | 9 - .../gps_l1_ca_dll_pll_tracking_fpga.cc | 8 - .../gps_l2_m_dll_pll_tracking_fpga.cc | 2 - .../adapters/gps_l5_dll_pll_tracking_fpga.cc | 9 - .../dll_pll_veml_tracking_fpga.cc | 3 +- .../tracking/libs/dll_pll_conf_fpga.cc | 1 - .../tracking/libs/dll_pll_conf_fpga.h | 1 - .../tracking/libs/fpga_multicorrelator.cc | 244 ++++-------------- .../tracking/libs/fpga_multicorrelator.h | 96 ++++--- 18 files changed, 149 insertions(+), 335 deletions(-) diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc index d434178ab..ca936e131 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc @@ -44,14 +44,6 @@ #include // for abs, pow, floor #include // for complex -// the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define QUANT_BITS_LOCAL_CODE 16 -#define SELECT_LSBits 0x0000FFFF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0xFFFF0000 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0xFFFFFFFF // Select a 20 bit word -#define SHL_CODE_BITS 65536 // shift left by 10 bits - GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( ConfigurationInterface* configuration, const std::string& role, @@ -166,10 +158,10 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga( // and package codes in a format that is ready to be written to the FPGA for (uint32_t i = 0; i < nsamples_total; i++) { - tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - local_code = (tmp & SELECT_LSBits) | ((tmp2 * SHL_CODE_BITS) & SELECT_MSBbits); // put together the real part and the imaginary part - fft_data = local_code & SELECT_ALL_CODE_BITS; + tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + local_code = (tmp & select_lsbits) | ((tmp2 * shl_code_bits) & select_msbits); // put together the real part and the imaginary part + fft_data = local_code & select_all_code_bits; d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; } } diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h index cb42896f3..1333619f6 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h @@ -153,6 +153,15 @@ public: void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: + + // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const uint32_t quant_bits_local_code = 16; + static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t shl_code_bits = 65536; // shift left by 10 bits + ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; size_t item_size_; diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc index e2a2a21f4..d1df3eca1 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc @@ -44,14 +44,6 @@ #include // for abs, pow, floor #include // for complex -// the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define QUANT_BITS_LOCAL_CODE 16 -#define SELECT_LSBits 0x0000FFFF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0xFFFF0000 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0xFFFFFFFF // Select a 20 bit word -#define SHL_CODE_BITS 65536 // shift left by 10 bits - GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, @@ -169,10 +161,10 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf // and package codes in a format that is ready to be written to the FPGA for (uint32_t i = 0; i < nsamples_total; i++) { - tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - local_code = (tmp & SELECT_LSBits) | ((tmp2 * SHL_CODE_BITS) & SELECT_MSBbits); // put together the real part and the imaginary part - fft_data = local_code & SELECT_ALL_CODE_BITS; + tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + local_code = (tmp & select_lsbits) | ((tmp2 * shl_code_bits) & select_msbits); // put together the real part and the imaginary part + fft_data = local_code & select_all_code_bits; d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; } } diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h index 2d6d5c8bc..b12c3ea8f 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h @@ -163,6 +163,15 @@ public: void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: + + // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const uint32_t quant_bits_local_code = 16; + static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t shl_code_bits = 65536; // shift left by 10 bits + ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; size_t item_size_; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc index 998da32a0..16c34b4cc 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc @@ -49,14 +49,6 @@ #define NUM_PRNs 32 -// the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define QUANT_BITS_LOCAL_CODE 16 -#define SELECT_LSBits 0x0000FFFF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0xFFFF0000 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0xFFFFFFFF // Select a 20 bit word -#define SHL_CODE_BITS 65536 // shift left by 10 bits - GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( ConfigurationInterface* configuration, const std::string& role, @@ -146,10 +138,10 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( // and package codes in a format that is ready to be written to the FPGA for (uint32_t i = 0; i < nsamples_total; i++) { - tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - local_code = (tmp & SELECT_LSBits) | ((tmp2 * SHL_CODE_BITS) & SELECT_MSBbits); // put together the real part and the imaginary part - fft_data = local_code & SELECT_ALL_CODE_BITS; + tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + local_code = (tmp & select_lsbits) | ((tmp2 * shl_code_bits) & select_msbits); // put together the real part and the imaginary part + fft_data = local_code & select_all_code_bits; d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; } } diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h index 836be98ec..47e62c90c 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h @@ -155,6 +155,16 @@ public: void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: + + // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const uint32_t quant_bits_local_code = 16; + static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t shl_code_bits = 65536; // shift left by 10 bits + + ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; size_t item_size_; diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc index fbafae22c..7c029ccca 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc @@ -49,15 +49,6 @@ #define NUM_PRNs 32 -// the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA -// expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. -#define QUANT_BITS_LOCAL_CODE 16 -#define SELECT_LSBits 0x0000FFFF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0xFFFF0000 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0xFFFFFFFF // Select a 20 bit word -#define SHL_CODE_BITS 65536 // shift left by 10 bits - - GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( ConfigurationInterface* configuration, const std::string& role, @@ -150,10 +141,10 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( // and package codes in a format that is ready to be written to the FPGA for (uint32_t i = 0; i < nsamples_total; i++) { - tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)); - local_code = (tmp & SELECT_LSBits) | ((tmp2 * SHL_CODE_BITS) & SELECT_MSBbits); // put together the real part and the imaginary part - fft_data = local_code & SELECT_ALL_CODE_BITS; + tmp = static_cast(floor(fft_codes_padded[i].real() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + tmp2 = static_cast(floor(fft_codes_padded[i].imag() * (pow(2, quant_bits_local_code - 1) - 1) / max)); + local_code = (tmp & select_lsbits) | ((tmp2 * shl_code_bits) & select_msbits); // put together the real part and the imaginary part + fft_data = local_code & select_all_code_bits; d_all_fft_codes_[i + (nsamples_total * (PRN - 1))] = fft_data; } } diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h index d6ab600fb..b5c8b4c50 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h @@ -154,6 +154,15 @@ public: void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: + + // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const uint32_t quant_bits_local_code = 16; + static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t shl_code_bits = 65536; // shift left by 10 bits + ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; size_t item_size_; diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc index a6acbc082..8077aaa5f 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc @@ -169,21 +169,12 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga( trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); - // int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - // if (FLAGS_max_lock_fail != 50) - // { - // max_lock_fail = FLAGS_max_lock_fail; - // } - // trk_param_fpga.max_lock_fail = max_lock_fail; - - // FPGA configuration parameters std::string default_device_name = "/dev/uio"; std::string device_name = configuration->property(role + ".devicename", default_device_name); trk_param_fpga.device_name = device_name; uint32_t device_base = configuration->property(role + ".device_base", 15); trk_param_fpga.device_base = device_base; - trk_param_fpga.multicorr_type = 1; // 0 -> 3 correlators, 1 -> 5 correlators //################# PRE-COMPUTE ALL THE CODES ################# uint32_t code_samples_per_chip = 2; diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index 6a5eeded8..7a2a30a0e 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -162,14 +162,6 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); - // int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - // if (FLAGS_max_lock_fail != 50) - // { - // max_lock_fail = FLAGS_max_lock_fail; - // } - // trk_param_fpga.max_lock_fail = max_lock_fail; - - d_data_codes = nullptr; // FPGA configuration parameters @@ -178,7 +170,6 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga( trk_param_fpga.device_name = device_name; uint32_t device_base = configuration->property(role + ".device_base", 27); trk_param_fpga.device_base = device_base; - trk_param_fpga.multicorr_type = 1; // 0 -> 3 correlators, 1 -> up to 5+1 correlators //################# PRE-COMPUTE ALL THE CODES ################# uint32_t code_samples_per_chip = 1; diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index cd8b623ef..be266a0dd 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -170,20 +170,12 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga( trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); - //int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - //if (FLAGS_max_lock_fail != 50) - // { - // max_lock_fail = FLAGS_max_lock_fail; - // } - //trk_param_fpga.max_lock_fail = max_lock_fail; - // FPGA configuration parameters std::string default_device_name = "/dev/uio"; std::string device_name = configuration->property(role + ".devicename", default_device_name); trk_param_fpga.device_name = device_name; uint32_t device_base = configuration->property(role + ".device_base", 3); trk_param_fpga.device_base = device_base; - trk_param_fpga.multicorr_type = 0; //multicorr_type : 0 -> 3 correlators, 1 -> 5 correlators //################# PRE-COMPUTE ALL THE CODES ################# d_ca_codes = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS * NUM_PRNs) * sizeof(int32_t), volk_gnsssdr_get_alignment())); diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc index 196bd1736..b26685ef2 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc @@ -116,8 +116,6 @@ GpsL2MDllPllTrackingFpga::GpsL2MDllPllTrackingFpga( trk_param_fpga.device_name = device_name; unsigned int device_base = configuration->property(role + ".device_base", 1); trk_param_fpga.device_base = device_base; - //unsigned int multicorr_type = configuration->property(role + ".multicorr_type", 0); - trk_param_fpga.multicorr_type = 0; //multicorr_type : 0 -> 3 correlators, 1 -> 5 correlators auto* ca_codes_f = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L2_M_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index 90c7ab19a..5e84b3ef2 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -171,21 +171,12 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( trk_param_fpga.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param_fpga.max_carrier_lock_fail); trk_param_fpga.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param_fpga.carrier_lock_th); - //int32_t max_lock_fail = configuration->property(role + ".max_lock_fail", 50); - //if (FLAGS_max_lock_fail != 50) - // { - // max_lock_fail = FLAGS_max_lock_fail; - // } - //trk_param_fpga.max_lock_fail = max_lock_fail; - - // FPGA configuration parameters std::string default_device_name = "/dev/uio"; std::string device_name = configuration->property(role + ".devicename", default_device_name); trk_param_fpga.device_name = device_name; uint32_t device_base = configuration->property(role + ".device_base", 27); trk_param_fpga.device_base = device_base; - trk_param_fpga.multicorr_type = 0; //multicorr_type : 0 -> 3 correlators, 1 -> 5 correlators //################# PRE-COMPUTE ALL THE CODES ################# uint32_t code_samples_per_chip = 1; auto code_length_chips = static_cast(GPS_L5I_CODE_LENGTH_CHIPS); 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 32ee343e4..9e4918736 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 @@ -451,8 +451,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & uint32_t device_base = trk_parameters.device_base; int32_t *ca_codes = trk_parameters.ca_codes; int32_t *data_codes = trk_parameters.data_codes; - uint32_t multicorr_type = trk_parameters.multicorr_type; - multicorrelator_fpga = std::make_shared(d_n_correlator_taps, device_name, device_base, ca_codes, data_codes, d_code_length_chips, trk_parameters.track_pilot, multicorr_type, d_code_samples_per_chip); + multicorrelator_fpga = std::make_shared(d_n_correlator_taps, device_name, device_base, ca_codes, data_codes, d_code_length_chips, trk_parameters.track_pilot, d_code_samples_per_chip); multicorrelator_fpga->set_output_vectors(d_correlator_outs, d_Prompt_Data); d_sample_counter_next = 0ULL; diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc index 28b5c243a..0f87846e7 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc @@ -78,7 +78,6 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga() signal[2] = '\0'; device_name = "/dev/uio"; device_base = 1U; - multicorr_type = 0U; code_length_chips = 0U; code_samples_per_chip = 0U; ca_codes = nullptr; diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h index a88d80212..1f1ca112c 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h +++ b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h @@ -82,7 +82,6 @@ public: char signal[3]; std::string device_name; uint32_t device_base; - uint32_t multicorr_type; uint32_t code_length_chips; uint32_t code_samples_per_chip; int32_t* ca_codes; diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.cc b/src/algorithms/tracking/libs/fpga_multicorrelator.cc index d3bdd114f..12ec3278d 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.cc +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.cc @@ -72,7 +72,8 @@ Fpga_Multicorrelator_8sc::Fpga_Multicorrelator_8sc(int32_t n_correlators, std::string device_name, uint32_t device_base, int32_t *ca_codes, int32_t *data_codes, uint32_t code_length_chips, bool track_pilot, - uint32_t multicorr_type, uint32_t code_samples_per_chip) + uint32_t code_samples_per_chip) + { d_n_correlators = n_correlators; d_device_name = std::move(device_name); @@ -114,7 +115,6 @@ Fpga_Multicorrelator_8sc::Fpga_Multicorrelator_8sc(int32_t n_correlators, d_code_length_chips = code_length_chips; d_ca_codes = ca_codes; d_data_codes = data_codes; - d_multicorr_type = multicorr_type; d_code_samples_per_chip = code_samples_per_chip; d_code_length_samples = d_code_length_chips * d_code_samples_per_chip; @@ -142,8 +142,8 @@ Fpga_Multicorrelator_8sc::~Fpga_Multicorrelator_8sc() uint64_t Fpga_Multicorrelator_8sc::read_sample_counter() { uint64_t sample_counter_tmp, sample_counter_msw_tmp; - sample_counter_tmp = d_map_base[SAMPLE_COUNTER_REG_ADDR_LSW]; - sample_counter_msw_tmp = d_map_base[SAMPLE_COUNTER_REG_ADDR_MSW]; + sample_counter_tmp = d_map_base[sample_counter_reg_addr_lsw]; + sample_counter_msw_tmp = d_map_base[sample_counter_reg_addr_msw]; sample_counter_msw_tmp = sample_counter_msw_tmp << 32; sample_counter_tmp = sample_counter_tmp + sample_counter_msw_tmp; // 2^32 return sample_counter_tmp; @@ -153,8 +153,8 @@ uint64_t Fpga_Multicorrelator_8sc::read_sample_counter() void Fpga_Multicorrelator_8sc::set_initial_sample(uint64_t samples_offset) { d_initial_sample_counter = samples_offset; - d_map_base[INITIAL_COUNTER_VALUE_REG_ADDR_LSW] = (d_initial_sample_counter & 0xFFFFFFFF); - d_map_base[INITIAL_COUNTER_VALUE_REG_ADDR_MSW] = (d_initial_sample_counter >> 32) & 0xFFFFFFFF; + d_map_base[initial_counter_value_reg_addr_lsw] = (d_initial_sample_counter & 0xFFFFFFFF); + d_map_base[initial_counter_value_reg_addr_msw] = (d_initial_sample_counter >> 32) & 0xFFFFFFFF; } @@ -212,18 +212,11 @@ void Fpga_Multicorrelator_8sc::Carrier_wipeoff_multicorrelator_resampler( // release secondary code indices, keep channel locked if (d_secondary_code_enabled == true) { - //printf("in the right place\n"); - // debug - force reset counter every time - //d_map_base[DROP_SAMPLES_REG_ADDR] = ENABLE_SECONDARY_CODE | INIT_SECONDARY_CODE_ADDRESSES; - d_map_base[DROP_SAMPLES_REG_ADDR] = ENABLE_SECONDARY_CODE; // keep secondary code enabled - - // //printf("do not enable secondary code on purpose\n"); - // d_map_base[DROP_SAMPLES_REG_ADDR] = 0; // block samples + d_map_base[drop_samples_reg_addr] = enable_secondary_code; // keep secondary code enabled } else { - //printf("in the wrong place\n"); - d_map_base[DROP_SAMPLES_REG_ADDR] = 0; // block samples + d_map_base[drop_samples_reg_addr] = 0; // block samples } Fpga_Multicorrelator_8sc::read_tracking_gps_results(); } @@ -308,9 +301,9 @@ uint32_t Fpga_Multicorrelator_8sc::fpga_acquisition_test_register( { uint32_t readval = 0; // write value to test register - d_map_base[TEST_REG_ADDR] = writeval; + d_map_base[test_reg_addr] = writeval; // read value from test register - readval = d_map_base[TEST_REG_ADDR]; + readval = d_map_base[test_reg_addr]; // return read value return readval; } @@ -320,21 +313,21 @@ void Fpga_Multicorrelator_8sc::fpga_configure_tracking_gps_local_code(int32_t PR { uint32_t k; - d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER; + d_map_base[prog_mems_addr] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER; for (k = 0; k < d_code_length_samples; k++) { - d_map_base[PROG_MEMS_ADDR] = d_ca_codes[(d_code_length_samples * (PRN - 1)) + k]; + d_map_base[prog_mems_addr] = d_ca_codes[(d_code_length_samples * (PRN - 1)) + k]; } if (d_track_pilot) { - d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER; + d_map_base[prog_mems_addr] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER; for (k = 0; k < d_code_length_samples; k++) { - d_map_base[PROG_MEMS_ADDR] = d_data_codes[(d_code_length_samples * (PRN - 1)) + k]; + d_map_base[prog_mems_addr] = d_data_codes[(d_code_length_samples * (PRN - 1)) + k]; } } - d_map_base[CODE_LENGTH_MINUS_1_REG_ADDR] = (d_code_length_samples)-1; // number of samples - 1 + d_map_base[code_length_minus_1_reg_addr] = (d_code_length_samples)-1; // number of samples - 1 } @@ -386,13 +379,13 @@ void Fpga_Multicorrelator_8sc::fpga_configure_code_parameters_in_fpga(void) { for (uint32_t i = 0; i < d_n_correlators; i++) { - d_map_base[INITIAL_INDEX_REG_BASE_ADDR + i] = d_initial_index[i]; - d_map_base[INITIAL_INTERP_COUNTER_REG_BASE_ADDR + i] = d_initial_interp_counter[i]; + d_map_base[initial_index_reg_base_addr + i] = d_initial_index[i]; + d_map_base[initial_interp_counter_reg_base_addr + i] = d_initial_interp_counter[i]; } if (d_track_pilot) { - d_map_base[INITIAL_INDEX_REG_BASE_ADDR + d_n_correlators] = d_initial_index[d_n_correlators]; - d_map_base[INITIAL_INTERP_COUNTER_REG_BASE_ADDR + d_n_correlators] = d_initial_interp_counter[d_n_correlators]; + d_map_base[initial_index_reg_base_addr + d_n_correlators] = d_initial_index[d_n_correlators]; + d_map_base[initial_interp_counter_reg_base_addr + d_n_correlators] = d_initial_interp_counter[d_n_correlators]; } } @@ -425,17 +418,17 @@ void Fpga_Multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(void) void Fpga_Multicorrelator_8sc::fpga_configure_signal_parameters_in_fpga(void) { - d_map_base[CODE_PHASE_STEP_CHIPS_NUM_REG_ADDR] = d_code_phase_step_chips_num; // code phase step + d_map_base[code_phase_step_chips_num_reg_addr] = d_code_phase_step_chips_num; // code phase step - d_map_base[CODE_PHASE_STEP_CHIPS_RATE] = d_code_phase_rate_step_chips_num; // code phase step rate + d_map_base[code_phase_step_chips_rate_reg_addr] = d_code_phase_rate_step_chips_num; // code phase step rate - d_map_base[NSAMPLES_MINUS_1_REG_ADDR] = d_correlator_length_samples - 1; // number of samples + d_map_base[nsamples_minus_1_reg_addr] = d_correlator_length_samples - 1; // number of samples - d_map_base[REM_CARR_PHASE_RAD_REG_ADDR] = d_rem_carr_phase_rad_int; // initial nco phase + d_map_base[rem_carr_phase_rad_reg_addr] = d_rem_carr_phase_rad_int; // initial nco phase - d_map_base[PHASE_STEP_RAD_REG_ADDR] = d_phase_step_rad_int; // nco phase step + d_map_base[phase_step_rad_reg_addr] = d_phase_step_rad_int; // nco phase step - d_map_base[PHASE_STEP_RATE_REG_ADDR] = d_carrier_phase_rate_step_rad_int; // nco phase step rate + d_map_base[phase_step_rate_reg_addr] = d_carrier_phase_rate_step_rad_int; // nco phase step rate } @@ -449,7 +442,7 @@ void Fpga_Multicorrelator_8sc::fpga_launch_multicorrelator_fpga(void) std::cerr << "Error launching the FPGA multicorrelator" << std::endl; } // writing 1 to reg 14 launches the tracking - d_map_base[START_FLAG_ADDR] = 1; + d_map_base[start_flag_addr] = 1; } @@ -460,14 +453,14 @@ void Fpga_Multicorrelator_8sc::read_tracking_gps_results(void) for (uint32_t k = 0; k < d_n_correlators; k++) { - readval_real = d_map_base[RESULT_REG_REAL_BASE_ADDR + k]; - readval_imag = d_map_base[RESULT_REG_IMAG_BASE_ADDR + k]; + readval_real = d_map_base[result_reg_real_base_addr + k]; + readval_imag = d_map_base[result_reg_imag_base_addr + k]; d_corr_out[k] = gr_complex(readval_real, readval_imag); } if (d_track_pilot) { - readval_real = d_map_base[RESULT_REG_REAL_BASE_ADDR + d_n_correlators]; - readval_imag = d_map_base[RESULT_REG_IMAG_BASE_ADDR + d_n_correlators]; + readval_real = d_map_base[result_reg_real_base_addr + d_n_correlators]; + readval_imag = d_map_base[result_reg_imag_base_addr + d_n_correlators]; d_Prompt_Data[0] = gr_complex(readval_real, readval_imag); } } @@ -476,8 +469,8 @@ void Fpga_Multicorrelator_8sc::read_tracking_gps_results(void) void Fpga_Multicorrelator_8sc::unlock_channel(void) { // unlock the channel to let the next samples go through - d_map_base[DROP_SAMPLES_REG_ADDR] = DROP_SAMPLES; // unlock the channel and disable secondary codes - d_map_base[STOP_TRACKING_REG_ADDR] = 1; // set the tracking module back to idle + d_map_base[drop_samples_reg_addr] = drop_samples; // unlock the channel and disable secondary codes + d_map_base[stop_tracking_reg_addr] = 1; // set the tracking module back to idle d_secondary_code_enabled = false; } @@ -497,7 +490,7 @@ void Fpga_Multicorrelator_8sc::close_device() void Fpga_Multicorrelator_8sc::lock_channel(void) { // lock the channel for processing - d_map_base[DROP_SAMPLES_REG_ADDR] = 0; // lock the channel + d_map_base[drop_samples_reg_addr] = 0; // lock the channel } @@ -506,24 +499,16 @@ void Fpga_Multicorrelator_8sc::set_secondary_code_lengths(uint32_t secondary_cod d_secondary_code_0_length = secondary_code_0_length; d_secondary_code_1_length = secondary_code_1_length; - // debug - //printf("warning extending the code length 0 to 20\n"); - //d_secondary_code_0_length = 20; - uint32_t secondary_code_length_0_minus_1 = d_secondary_code_0_length - 1; uint32_t secondary_code_length_1_minus_1 = d_secondary_code_1_length - 1; - d_map_base[SECONDARY_CODE_LENGTHS_REG_ADDR] = secondary_code_length_1_minus_1 * 256 + secondary_code_length_0_minus_1; - - //std::cout << "setting secondary code lengths : \n"; - //std::cout << "initialized correlator 1 sec code length = " << d_secondary_code_1_length << " correlator 0 sec code length = " << d_secondary_code_0_length << std::endl; + d_map_base[secondary_code_lengths_reg_addr] = secondary_code_length_1_minus_1 * 256 + secondary_code_length_0_minus_1; } void Fpga_Multicorrelator_8sc::update_secondary_code_length(uint32_t first_length_secondary_code, uint32_t next_length_secondary_code) { - d_map_base[FIRST_PRN_LENGTH_MINUS_1_REG_ADDR] = first_length_secondary_code - 1; - d_map_base[NEXT_PRN_LENGTH_MINUS_1_REG_ADDR] = next_length_secondary_code - 1; - //std::cout << " first_length_secondary_code = " << first_length_secondary_code << " next_length_secondary_code = " << next_length_secondary_code << " sum = " << first_length_secondary_code + next_length_secondary_code << std::endl; + d_map_base[first_prn_length_minus_1_reg_addr] = first_length_secondary_code - 1; + d_map_base[next_prn_length_minus_1_reg_addr] = next_length_secondary_code - 1; } void Fpga_Multicorrelator_8sc::initialize_secondary_code(uint32_t secondary_code, std::string *secondary_code_string) @@ -533,67 +518,26 @@ void Fpga_Multicorrelator_8sc::initialize_secondary_code(uint32_t secondary_code if (secondary_code == 0) { secondary_code_length = d_secondary_code_0_length; - reg_addr = PROG_SECONDARY_CODE_0_DATA_REG_ADDR; + reg_addr = prog_secondary_code_0_data_reg_addr; } else { secondary_code_length = d_secondary_code_1_length; - reg_addr = PROG_SECONDARY_CODE_1_DATA_REG_ADDR; + reg_addr = prog_secondary_code_1_data_reg_addr; } Fpga_Multicorrelator_8sc::write_secondary_code(secondary_code_length, secondary_code_string, reg_addr); } -//void Fpga_Multicorrelator_8sc::initialize_secondary_codes(bool track_pilot, -// uint32_t secondary_code_length_data, std::string *secondary_code_string_data, -// uint32_t secondary_code_length_pilot, std::string *secondary_code_string_pilot) -//{ -// if (track_pilot) -// { -// // write secondary_code_length_pilot | secondary_code_length_data << 8 -// d_map_base[SECONDARY_CODE_LENGTHS_REG_ADDR] = secondary_code_length_pilot + secondary_code_length_data*256; -// -// // write pilot secondary code -// Fpga_Multicorrelator_8sc::write_secondary_code(secondary_code_length_pilot, secondary_code_string_pilot, PROG_SECONDARY_CODE_0_DATA_REG_ADDR); -// -// // write data secondary code -// Fpga_Multicorrelator_8sc::write_secondary_code(secondary_code_length_data, secondary_code_string_pilot, PROG_SECONDARY_CODE_1_DATA_REG_ADDR); -// -// } -// else -// { -// d_map_base[SECONDARY_CODE_LENGTHS_REG_ADDR] = secondary_code_length_data; -// -// // write data secondary code -// Fpga_Multicorrelator_8sc::write_secondary_code(secondary_code_length_data, secondary_code_string_pilot, PROG_SECONDARY_CODE_0_DATA_REG_ADDR); -// } -// -// std::cout << "going to print string " << std::endl; -// std::cout << secondary_code_string_data << std::endl; -// -// -//} - void Fpga_Multicorrelator_8sc::write_secondary_code(uint32_t secondary_code_length, std::string *secondary_code_string, uint32_t reg_addr) { - uint32_t num_words = ceil(((float)secondary_code_length) / SECONDARY_CODE_WORD_SIZE); - uint32_t last_word_size = secondary_code_length % SECONDARY_CODE_WORD_SIZE; - //uint32_t initial_pointer; + uint32_t num_words = ceil(((float)secondary_code_length) / secondary_code_word_size); + uint32_t last_word_size = secondary_code_length % secondary_code_word_size; if (last_word_size == 0) { - last_word_size = SECONDARY_CODE_WORD_SIZE; + last_word_size = secondary_code_word_size; } - // debug - //std::cout << "secondary_code_length = " << secondary_code_length << std::endl; - //std::cout << "secondary code string = " << *secondary_code_string << std::endl; - //std::cout << "reg_addr = " << reg_addr << std::endl; - - - // debug - //std::cout << "num_words = " << num_words << std::endl; - //std::cout << "last_word_size = " << last_word_size << std::endl; - uint32_t write_val = 0U; uint32_t pow_k; @@ -602,134 +546,42 @@ void Fpga_Multicorrelator_8sc::write_secondary_code(uint32_t secondary_code_leng { for (mem_addr = 0; mem_addr < num_words - 1; mem_addr++) { - //std::cout << "------------------------------------------------------ going to write word " << mem_addr << std::endl; write_val = 0U; pow_k = 1; - for (unsigned int k = 0; k < SECONDARY_CODE_WORD_SIZE; k++) + for (unsigned int k = 0; k < secondary_code_word_size; k++) { - // debug - //std::cout << "reading bit position = " << mem_addr*SECONDARY_CODE_WORD_SIZE + k << std::endl; - //std::cout << "bit shift = " << pow_k << std::endl; - - std::string string_tmp(1, secondary_code_string->at(mem_addr * SECONDARY_CODE_WORD_SIZE + k)); + std::string string_tmp(1, secondary_code_string->at(mem_addr * secondary_code_word_size + k)); write_val = write_val | std::stoi(string_tmp) * pow_k; - - // debug - //std::cout << "computing bit k = " << k << " bit k value = "<< std::stoi(string_tmp) << std::endl; - //std::cout << "computing bit k displaced = " << std::stoi(string_tmp)*pow_k << std::endl; - //std::cout << "write val = " << write_val << std::endl; - pow_k = pow_k * 2; } - - //std::cout << "writing secondary code reg addr " << reg_addr << "secondary code value " << write_val << std::endl; - - - // debug - //write_val = 0; - - write_val = write_val | mem_addr * SECONDARY_CODE_ADDR_BITS | SECONDARY_CODE_WR_STROBE; + write_val = write_val | mem_addr * secondary_code_addr_bits | secondary_code_wr_strobe; d_map_base[reg_addr] = write_val; - - //std::cout << "writing fpga register value " << write_val << std::endl; - - - // debug - // std::cout << "wrote word " << mem_addr << "value is "; - // while (write_val) { - // if (write_val & 1) - // printf("1"); - // else - // printf("0"); - // - // write_val >>= 1; - // } - // printf("\n"); } } write_val = 0U; pow_k = 1; mem_addr = num_words - 1; - //std::cout << "------------------------------------------------------ going to write word " << mem_addr << std::endl; - for (unsigned int k = 0; k < last_word_size; k++) { - // debug - //std::cout << "reading bit position = " << mem_addr*SECONDARY_CODE_WORD_SIZE + k << std::endl; - //std::cout << "bit shift = " << pow_k << std::endl; - - std::string string_tmp(1, secondary_code_string->at(mem_addr * SECONDARY_CODE_WORD_SIZE + k)); + std::string string_tmp(1, secondary_code_string->at(mem_addr * secondary_code_word_size + k)); write_val = write_val | std::stoi(string_tmp) * pow_k; - - // debug - //std::cout << "computing bit k = " << k << " bit k value = "<< std::stoi(string_tmp) << std::endl; - //std::cout << "computing bit k displaced = " << std::stoi(string_tmp)*pow_k << std::endl; - //std::cout << "write val = " << write_val << std::endl; - pow_k = pow_k * 2; } - // debug - //write_val = 0; - - //std::cout << "writing secondary code reg addr " << reg_addr << "secondary code value " << write_val << std::endl; - - write_val = write_val | (mem_addr * SECONDARY_CODE_ADDR_BITS) | (SECONDARY_CODE_WR_STROBE); + write_val = write_val | (mem_addr * secondary_code_addr_bits) | (secondary_code_wr_strobe); d_map_base[reg_addr] = write_val; - //std::cout << "writing fpga register value " << write_val << std::endl; - - - // // debug - // write_val = write_val | 705200; - // d_map_base[reg_addr] = write_val; - // printf("warning : extending the code length to 20\n"); - // std::cout << "writing fpga register value " << write_val << std::endl; - - // // debug - // //write_val = (SECONDARY_CODE_WR_STROBE) | 0x00055400; - // write_val = (SECONDARY_CODE_WR_STROBE) | 0x00000155; - // d_map_base[reg_addr] = write_val; - // for (unsigned int k=1;k<5;k++) - // { - // write_val = (k*SECONDARY_CODE_ADDR_BITS) | (SECONDARY_CODE_WR_STROBE) | 0x00055555; - // d_map_base[reg_addr] = write_val; - // } - - // // debug - // std::cout << "wrote word " << mem_addr << " value is " << write_val << " = "; - // while (write_val) { - // if (write_val & 1) - // printf("1"); - // else - // printf("0"); - // - // write_val >>= 1; - // } - // printf("\n"); - //printf("\n=============================================================================* END OF THIS\n"); } -//void Fpga_Multicorrelator_8sc::init_secondary_code_indices(void) -//{ -// d_map_base[DROP_SAMPLES_REG_ADDR] = 5; // leave channel unlocked, and init -//} - void Fpga_Multicorrelator_8sc::enable_secondary_codes() { - d_map_base[DROP_SAMPLES_REG_ADDR] = INIT_SECONDARY_CODE_ADDRESSES | ENABLE_SECONDARY_CODE; // enable secondary codes and clear secondary code indices + d_map_base[drop_samples_reg_addr] = init_secondary_code_addresses | enable_secondary_code; // enable secondary codes and clear secondary code indices d_secondary_code_enabled = true; - //std::cout << "enabling secondary codes d_map_base[DROP_SAMPLES_REG_ADDR] = " << (INIT_SECONDARY_CODE_ADDRESSES | ENABLE_SECONDARY_CODE) << std::endl; - - // // debug - // printf("do not enable secondary code on purpose\n"); - // d_map_base[DROP_SAMPLES_REG_ADDR] = 0; } void Fpga_Multicorrelator_8sc::disable_secondary_codes() { // this function is to be called before starting the tracking process in order to disable the secondary codes by default - //printf("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx disabling secondary codes in fpga\n"); - d_map_base[DROP_SAMPLES_REG_ADDR] = DROP_SAMPLES; + d_map_base[drop_samples_reg_addr] = drop_samples; } diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.h b/src/algorithms/tracking/libs/fpga_multicorrelator.h index 1983215fb..4712c7d92 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.h +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.h @@ -29,7 +29,7 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . + * along with GNSS-SDR. If not, see . * * ------------------------------------------------------------------------- */ @@ -40,45 +40,8 @@ #include #include -// FPGA register addresses -// write addresses -#define CODE_PHASE_STEP_CHIPS_NUM_REG_ADDR 0 -#define INITIAL_INDEX_REG_BASE_ADDR 1 -#define INITIAL_INTERP_COUNTER_REG_BASE_ADDR 7 -#define NSAMPLES_MINUS_1_REG_ADDR 13 -#define CODE_LENGTH_MINUS_1_REG_ADDR 14 -#define REM_CARR_PHASE_RAD_REG_ADDR 15 -#define PHASE_STEP_RAD_REG_ADDR 16 -#define PROG_MEMS_ADDR 17 -#define DROP_SAMPLES_REG_ADDR 18 -#define INITIAL_COUNTER_VALUE_REG_ADDR_LSW 19 -#define INITIAL_COUNTER_VALUE_REG_ADDR_MSW 20 -#define CODE_PHASE_STEP_CHIPS_RATE 21 -#define PHASE_STEP_RATE_REG_ADDR 22 -#define STOP_TRACKING_REG_ADDR 23 -#define INT_ON_RST_REG_ADDR 24 // cause interrupt on reset to prevent deadlock -#define SECONDARY_CODE_LENGTHS_REG_ADDR 25 -#define PROG_SECONDARY_CODE_0_DATA_REG_ADDR 26 -#define PROG_SECONDARY_CODE_1_DATA_REG_ADDR 27 -#define FIRST_PRN_LENGTH_MINUS_1_REG_ADDR 28 -#define NEXT_PRN_LENGTH_MINUS_1_REG_ADDR 29 -#define START_FLAG_ADDR 30 -// read-write addresses -#define TEST_REG_ADDR 31 -// read addresses -#define RESULT_REG_REAL_BASE_ADDR 1 -#define RESULT_REG_IMAG_BASE_ADDR 7 -#define SAMPLE_COUNTER_REG_ADDR_LSW 13 -#define SAMPLE_COUNTER_REG_ADDR_MSW 14 -// FPGA-related constants -#define SECONDARY_CODE_WORD_SIZE 20 // the secondary codes are written in to the FPGA in words of SECONDARY_CODE_WORD_SIZE bits -#define SECONDARY_CODE_WR_STROBE 0x800000 // write strobe position in the secondary code write register -#define SECONDARY_CODE_ADDR_BITS 0x100000 // memory address position in the secondary code write register -#define DROP_SAMPLES 1 // bit 0 of DROP_SAMPLES_REG_ADDR -#define ENABLE_SECONDARY_CODE 2 // bit 1 of DROP_SAMPLES_REG_ADDR -#define INIT_SECONDARY_CODE_ADDRESSES 4 // bit 2 of DROP_SAMPLES_REG_ADDR /*! * \brief Class that implements carrier wipe-off and correlators. @@ -87,7 +50,7 @@ class Fpga_Multicorrelator_8sc { public: Fpga_Multicorrelator_8sc(int32_t n_correlators, std::string device_name, - uint32_t device_base, int32_t *ca_codes, int32_t *data_codes, uint32_t code_length_chips, bool track_pilot, uint32_t multicorr_type, uint32_t code_samples_per_chip); + uint32_t device_base, int32_t *ca_codes, int32_t *data_codes, uint32_t code_length_chips, bool track_pilot, uint32_t code_samples_per_chip); ~Fpga_Multicorrelator_8sc(); void set_output_vectors(gr_complex *corr_out, gr_complex *Prompt_Data); void set_local_code_and_taps( @@ -105,18 +68,54 @@ public: uint64_t read_sample_counter(); void lock_channel(void); void unlock_channel(void); - // void initialize_secondary_codes(bool track_pilot, - // uint32_t secondary_code_length_data, std::string *secondary_code_string_data, - // uint32_t secondary_code_length_pilot, std::string *secondary_code_string_pilot); void set_secondary_code_lengths(uint32_t secondary_code_0_length, uint32_t secondary_code_1_length); void initialize_secondary_code(uint32_t secondary_code, std::string *secondary_code_string); void update_secondary_code_length(uint32_t first_length_secondary_code, uint32_t next_length_secondary_code); void enable_secondary_codes(); void disable_secondary_codes(); - // void init_secondary_code_indices(); private: + + // FPGA register addresses + + // write addresses + static const uint32_t code_phase_step_chips_num_reg_addr = 0; + static const uint32_t initial_index_reg_base_addr = 1; + static const uint32_t initial_interp_counter_reg_base_addr = 7; + static const uint32_t nsamples_minus_1_reg_addr = 13; + static const uint32_t code_length_minus_1_reg_addr = 14; + static const uint32_t rem_carr_phase_rad_reg_addr = 15; + static const uint32_t phase_step_rad_reg_addr = 16; + static const uint32_t prog_mems_addr = 17; + static const uint32_t drop_samples_reg_addr = 18; + static const uint32_t initial_counter_value_reg_addr_lsw = 19; + static const uint32_t initial_counter_value_reg_addr_msw = 20; + static const uint32_t code_phase_step_chips_rate_reg_addr = 21; + static const uint32_t phase_step_rate_reg_addr = 22; + static const uint32_t stop_tracking_reg_addr = 23; + static const uint32_t secondary_code_lengths_reg_addr = 25; + static const uint32_t prog_secondary_code_0_data_reg_addr = 26; + static const uint32_t prog_secondary_code_1_data_reg_addr = 27; + static const uint32_t first_prn_length_minus_1_reg_addr = 28; + static const uint32_t next_prn_length_minus_1_reg_addr = 29; + static const uint32_t start_flag_addr = 30; + // read-write addresses + static const uint32_t test_reg_addr = 31; + // read addresses + static const uint32_t result_reg_real_base_addr = 1; + static const uint32_t result_reg_imag_base_addr = 7; + static const uint32_t sample_counter_reg_addr_lsw = 13; + static const uint32_t sample_counter_reg_addr_msw = 14; + + // FPGA-related constants + static const uint32_t secondary_code_word_size = 20; // the secondary codes are written in to the FPGA in words of secondary_code_word_size bits + static const uint32_t secondary_code_wr_strobe = 0x800000; // write strobe position in the secondary code write register + static const uint32_t secondary_code_addr_bits = 0x100000; // memory address position in the secondary code write register + static const uint32_t drop_samples = 1; // bit 0 of drop_samples_reg_addr + static const uint32_t enable_secondary_code = 2; // bit 1 of drop_samples_reg_addr + static const uint32_t init_secondary_code_addresses = 4; // bit 2 of drop_samples_reg_addr + gr_complex *d_corr_out; gr_complex *d_Prompt_Data; float *d_shifts_chips; @@ -138,6 +137,8 @@ private: float d_rem_carrier_phase_in_rad; float d_phase_step_rad; float d_carrier_phase_rate_step_rad; + uint32_t d_code_samples_per_chip; + bool d_track_pilot; // configuration data computed in the format that the FPGA expects uint32_t *d_initial_index; @@ -153,18 +154,15 @@ private: std::string d_device_name; uint32_t d_device_base; + // PRN codes int32_t *d_ca_codes; int32_t *d_data_codes; - uint32_t d_code_samples_per_chip; - bool d_track_pilot; - - uint32_t d_multicorr_type; - + // secondary code configuration uint32_t d_secondary_code_0_length; uint32_t d_secondary_code_1_length; - bool d_secondary_code_enabled; + // private functions uint32_t fpga_acquisition_test_register(uint32_t writeval); void fpga_configure_tracking_gps_local_code(int32_t PRN); From 6ee9fedd7686d054818eb349cd56b8221cc05bb1 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Tue, 23 Jul 2019 15:20:59 +0200 Subject: [PATCH 18/22] moved #defines to static consts + used clang-format + removed code that was commented out --- .../gps_l1_ca_pcps_acquisition_fpga.cc | 2 - .../gps_l1_ca_pcps_acquisition_fpga.h | 7 ++- .../gps_l2_m_pcps_acquisition_fpga.cc | 7 --- .../adapters/gps_l2_m_pcps_acquisition_fpga.h | 7 +++ .../adapters/gps_l5i_pcps_acquisition_fpga.cc | 2 - .../adapters/gps_l5i_pcps_acquisition_fpga.h | 8 +-- .../gnuradio_blocks/pcps_acquisition_fpga.cc | 57 ------------------- .../acquisition/libs/fpga_acquisition.cc | 16 ------ .../acquisition/libs/fpga_acquisition.h | 15 +++++ .../signal_source/libs/fpga_switch.cc | 5 -- .../signal_source/libs/fpga_switch.h | 6 +- .../adapters/gps_l5_dll_pll_tracking_fpga.cc | 2 - .../adapters/gps_l5_dll_pll_tracking_fpga.h | 1 + .../tracking/libs/fpga_multicorrelator.cc | 45 +++++---------- .../tracking/libs/fpga_multicorrelator.h | 22 ++++--- src/core/libs/gnss_sdr_fpga_sample_counter.cc | 8 +-- src/core/libs/gnss_sdr_fpga_sample_counter.h | 3 + 17 files changed, 69 insertions(+), 144 deletions(-) diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc index 16c34b4cc..f66c4a2c7 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc @@ -47,8 +47,6 @@ #include // for abs, pow, floor #include // for complex -#define NUM_PRNs 32 - GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( ConfigurationInterface* configuration, const std::string& role, diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h index 47e62c90c..b38f123df 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h @@ -155,14 +155,15 @@ public: void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: + static const uint32_t NUM_PRNs = 32; // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. static const uint32_t quant_bits_local_code = 16; static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word - static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word - static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word - static const uint32_t shl_code_bits = 65536; // shift left by 10 bits + static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t shl_code_bits = 65536; // shift left by 10 bits ConfigurationInterface* configuration_; diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc index f01d49860..23a5d9f1f 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc @@ -46,13 +46,6 @@ #include // for abs, pow, floor #include // for complex -#define NUM_PRNs 32 -#define QUANT_BITS_LOCAL_CODE 16 -#define SELECT_LSBits 0x0000FFFF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0xFFFF0000 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0xFFFFFFFF // Select a 20 bit word -#define SHL_CODE_BITS 65536 // shift left by 10 bits - GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga( ConfigurationInterface* configuration, const std::string& role, diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.h index d560f04d3..e16419989 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.h @@ -156,6 +156,13 @@ public: void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: + static const uint32_t NUM_PRNs = 32; + static const uint32_t QUANT_BITS_LOCAL_CODE = 16; + static const uint32_t SELECT_LSBits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t SELECT_MSBbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t SELECT_ALL_CODE_BITS = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t SHL_CODE_BITS = 65536; // shift left by 10 bits + ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; std::string item_type_; diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc index 7c029ccca..3ecd513a5 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc @@ -47,8 +47,6 @@ #include // for abs, pow, floor #include // for complex -#define NUM_PRNs 32 - GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga( ConfigurationInterface* configuration, const std::string& role, diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h index b5c8b4c50..f4d7bdcb7 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h @@ -41,7 +41,6 @@ #include #include - class ConfigurationInterface; /*! @@ -154,14 +153,15 @@ public: void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: + static const uint32_t NUM_PRNs = 32; // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. static const uint32_t quant_bits_local_code = 16; static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word - static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word - static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word - static const uint32_t shl_code_bits = 65536; // shift left by 10 bits + static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t shl_code_bits = 65536; // shift left by 10 bits ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc index 76dd4e42b..521b5e767 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc @@ -40,8 +40,6 @@ #include // for move -#define AQ_DOWNSAMPLING_DELAY 40 // delay due to the downsampling filter in the acquisition - pcps_acquisition_fpga_sptr pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_) { return pcps_acquisition_fpga_sptr(new pcps_acquisition_fpga(std::move(conf_))); @@ -131,54 +129,6 @@ void pcps_acquisition_fpga::set_state(int32_t state) void pcps_acquisition_fpga::send_positive_acquisition() { - // debug L5 - // d_gnss_synchro->Acq_delay_samples = 2694; - // d_gnss_synchro->Acq_doppler_hz = 2650; - // d_gnss_synchro->Acq_samplestamp_samples = 56500224; - // d_gnss_synchro->Flag_valid_word = 0; - // d_gnss_synchro->Flag_valid_pseudorange = 0; - // d_gnss_synchro->Flag_valid_symbol_output = 0; - // d_gnss_synchro->Flag_valid_acquisition = 0; - - // d_gnss_synchro->Acq_delay_samples = 10846; - // d_gnss_synchro->Acq_doppler_hz = 2575; - // d_gnss_synchro->Acq_samplestamp_samples = 399605760; - // d_gnss_synchro->Flag_valid_word = 0; - // d_gnss_synchro->Flag_valid_pseudorange = 0; - // d_gnss_synchro->Flag_valid_symbol_output = 0; - // d_gnss_synchro->Flag_valid_acquisition = 0; - - // if (d_channel == 0) - // { - // d_gnss_synchro->Acq_delay_samples = 401; - // d_gnss_synchro->Acq_doppler_hz = 2650; - // d_gnss_synchro->Acq_samplestamp_samples = 96591872; - // d_gnss_synchro->Flag_valid_word = 0; - // d_gnss_synchro->Flag_valid_pseudorange = 0; - // d_gnss_synchro->Flag_valid_symbol_output = 0; - // d_gnss_synchro->Flag_valid_acquisition = 0; - - // d_gnss_synchro->Acq_delay_samples = 1505; - // d_gnss_synchro->Acq_doppler_hz = 2575; - // d_gnss_synchro->Acq_samplestamp_samples = 194265553; - // d_gnss_synchro->Flag_valid_word = 0; - // d_gnss_synchro->Flag_valid_pseudorange = 0; - // d_gnss_synchro->Flag_valid_symbol_output = 0; - // d_gnss_synchro->Flag_valid_acquisition = 0; - - // } - - - // debug E5a - // d_gnss_synchro->Acq_delay_samples = 2012; - // d_gnss_synchro->Acq_doppler_hz = -1125; - // d_gnss_synchro->Acq_samplestamp_samples = 363462656; - // d_gnss_synchro->Flag_valid_word = 0; - // d_gnss_synchro->Flag_valid_pseudorange = 0; - // d_gnss_synchro->Flag_valid_symbol_output = 0; - // d_gnss_synchro->Flag_valid_acquisition = 0; - - // Declare positive acquisition using a message port // 0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL DLOG(INFO) << "positive acquisition" @@ -276,13 +226,6 @@ void pcps_acquisition_fpga::acquisition_core(uint32_t num_doppler_bins, uint32_t } } - // debug - // if (d_test_statistics > d_threshold) - // { - // printf("firstpeak = %f, secondpeak = %f, test_statistics = %f reported block exp = %d PRN = %d inext = %d, initial_sample = %ld doppler = %d\n", firstpeak, secondpeak, d_test_statistics, (int)total_block_exp, (int)d_gnss_synchro->PRN, (int)indext, (long int)initial_sample, (int)doppler); - // printf("doppler_min = %d doppler_step = %d num_doppler_bins = %d\n", (int)doppler_min, (int)doppler_step, (int)num_doppler_bins); - // } - d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); d_sample_counter = initial_sample; diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.cc b/src/algorithms/acquisition/libs/fpga_acquisition.cc index c7d95c23e..7168113ef 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.cc +++ b/src/algorithms/acquisition/libs/fpga_acquisition.cc @@ -44,22 +44,6 @@ #include // for move -// FPGA register parameters -#define PAGE_SIZE 0x10000 // default page size for the multicorrelator memory map -#define RESET_ACQUISITION 2 // command to reset the multicorrelator -#define LAUNCH_ACQUISITION 1 // command to launch the multicorrelator -#define TEST_REG_SANITY_CHECK 0x55AA // value to check the presence of the test register (to detect the hw) -#define LOCAL_CODE_CLEAR_MEM 0x10000000 // command to clear the internal memory of the multicorrelator -#define MEM_LOCAL_CODE_WR_ENABLE 0x0C000000 // command to enable the ENA and WR pins of the internal memory of the multicorrelator -#define POW_2_2 4 // 2^2 (used for the conversion of floating point numbers to integers) -#define POW_2_31 2147483648 // 2^31 (used for the conversion of floating point numbers to integers) - -#define SELECT_LSBits 0x0000FFFF // Select the 10 LSbits out of a 20-bit word -#define SELECT_MSBbits 0xFFFF0000 // Select the 10 MSbits out of a 20-bit word -#define SELECT_ALL_CODE_BITS 0xFFFFFFFF // Select a 20 bit word -#define SHL_CODE_BITS 65536 // shift left by 10 bits - - #ifndef TEMP_FAILURE_RETRY #define TEMP_FAILURE_RETRY(exp) \ ({ \ diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.h b/src/algorithms/acquisition/libs/fpga_acquisition.h index 0e8b1926d..2a85aa7ab 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.h +++ b/src/algorithms/acquisition/libs/fpga_acquisition.h @@ -116,6 +116,21 @@ public: void close_device(); private: + // FPGA register parameters + static const uint32_t PAGE_SIZE = 0x10000; // default page size for the multicorrelator memory map + static const uint32_t RESET_ACQUISITION = 2; // command to reset the multicorrelator + static const uint32_t LAUNCH_ACQUISITION = 1; // command to launch the multicorrelator + static const uint32_t TEST_REG_SANITY_CHECK = 0x55AA; // value to check the presence of the test register (to detect the hw) + static const uint32_t LOCAL_CODE_CLEAR_MEM = 0x10000000; // command to clear the internal memory of the multicorrelator + static const uint32_t MEM_LOCAL_CODE_WR_ENABLE = 0x0C000000; // command to enable the ENA and WR pins of the internal memory of the multicorrelator + static const uint32_t POW_2_2 = 4; // 2^2 (used for the conversion of floating point numbers to integers) + static const uint32_t POW_2_31 = 2147483648; // 2^31 (used for the conversion of floating point numbers to integers) + + static const uint32_t SELECT_LSBits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t SELECT_MSBbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t SELECT_ALL_CODE_BITS = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t SHL_CODE_BITS = 65536; // shift left by 10 bits + int64_t d_fs_in; // data related to the hardware module and the driver int32_t d_fd; // driver descriptor diff --git a/src/algorithms/signal_source/libs/fpga_switch.cc b/src/algorithms/signal_source/libs/fpga_switch.cc index 5dec90e55..d189368b3 100644 --- a/src/algorithms/signal_source/libs/fpga_switch.cc +++ b/src/algorithms/signal_source/libs/fpga_switch.cc @@ -40,11 +40,6 @@ #include // for cout, endl #include // for mmap - -// constants -const size_t PAGE_SIZE = 0x10000; -const uint32_t TEST_REGISTER_TRACK_WRITEVAL = 0x55AA; - Fpga_Switch::Fpga_Switch(const std::string &device_name) { if ((d_device_descriptor = open(device_name.c_str(), O_RDWR | O_SYNC)) == -1) diff --git a/src/algorithms/signal_source/libs/fpga_switch.h b/src/algorithms/signal_source/libs/fpga_switch.h index c6a67c5fc..51d198fa3 100644 --- a/src/algorithms/signal_source/libs/fpga_switch.h +++ b/src/algorithms/signal_source/libs/fpga_switch.h @@ -39,8 +39,6 @@ #include -#define MAX_LENGTH_DEVICEIO_NAME 50 - class Fpga_Switch { public: @@ -49,6 +47,10 @@ public: void set_switch_position(int32_t switch_position); private: + static const size_t PAGE_SIZE = 0x10000; + static const uint32_t TEST_REGISTER_TRACK_WRITEVAL = 0x55AA; + static const uint32_t MAX_LENGTH_DEVICEIO_NAME = 50; + int d_device_descriptor; // driver descriptor volatile unsigned* d_map_base; // driver memory map diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index 5e84b3ef2..cce524561 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -47,8 +47,6 @@ #include #include -#define NUM_PRNs 32 // number of PRNS - GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga( ConfigurationInterface *configuration, const std::string &role, unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h index 80aff551e..af3a58802 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h @@ -96,6 +96,7 @@ public: void stop_tracking() override; private: + static const uint32_t NUM_PRNs = 32; // total number of PRNs // the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.cc b/src/algorithms/tracking/libs/fpga_multicorrelator.cc index 12ec3278d..66d569f0a 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.cc +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.cc @@ -43,20 +43,6 @@ #include // for PROT_READ, PROT_WRITE, MAP_SHARED #include -// FPGA register access constants -#define PAGE_SIZE 0x10000 -#define MAX_LENGTH_DEVICEIO_NAME 50 -#define CODE_RESAMPLER_NUM_BITS_PRECISION 20 -#define CODE_PHASE_STEP_CHIPS_NUM_NBITS CODE_RESAMPLER_NUM_BITS_PRECISION -#define pwrtwo(x) (1 << (x)) -#define MAX_CODE_RESAMPLER_COUNTER pwrtwo(CODE_PHASE_STEP_CHIPS_NUM_NBITS) // 2^CODE_PHASE_STEP_CHIPS_NUM_NBITS -#define PHASE_CARR_MAX 2147483648 // 2^(31) The phase is represented as a 32-bit vector in 1.31 format -#define PHASE_CARR_MAX_div_PI 683565275.5764316 // 2^(31)/pi -#define TWO_PI 6.283185307179586 -#define LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT 0x20000000 -#define LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER 0x10000000 -#define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000 -#define TEST_REGISTER_TRACK_WRITEVAL 0x55AA #ifndef TEMP_FAILURE_RETRY #define TEMP_FAILURE_RETRY(exp) \ ({ \ @@ -246,7 +232,7 @@ bool Fpga_Multicorrelator_8sc::free() void Fpga_Multicorrelator_8sc::set_channel(uint32_t channel) { - char device_io_name[MAX_LENGTH_DEVICEIO_NAME]; // driver io name + char device_io_name[max_length_deviceio_name]; // driver io name d_channel = channel; // open the device corresponding to the assigned channel @@ -256,9 +242,9 @@ void Fpga_Multicorrelator_8sc::set_channel(uint32_t channel) devicebasetemp << numdevice; mergedname = d_device_name + devicebasetemp.str(); - if (mergedname.size() > MAX_LENGTH_DEVICEIO_NAME) + if (mergedname.size() > max_length_deviceio_name) { - mergedname = mergedname.substr(0, MAX_LENGTH_DEVICEIO_NAME); + mergedname = mergedname.substr(0, max_length_deviceio_name); } mergedname.copy(device_io_name, mergedname.size() + 1); @@ -270,7 +256,7 @@ void Fpga_Multicorrelator_8sc::set_channel(uint32_t channel) LOG(WARNING) << "Cannot open deviceio" << device_io_name; std::cout << "Cannot open deviceio" << device_io_name << std::endl; } - d_map_base = reinterpret_cast(mmap(nullptr, PAGE_SIZE, + d_map_base = reinterpret_cast(mmap(nullptr, page_size, PROT_READ | PROT_WRITE, MAP_SHARED, d_device_descriptor, 0)); if (d_map_base == reinterpret_cast(-1)) @@ -281,7 +267,7 @@ void Fpga_Multicorrelator_8sc::set_channel(uint32_t channel) } // sanity check: check test register - uint32_t writeval = TEST_REGISTER_TRACK_WRITEVAL; + uint32_t writeval = test_register_track_writeval; uint32_t readval; readval = Fpga_Multicorrelator_8sc::fpga_acquisition_test_register(writeval); if (writeval != readval) @@ -313,14 +299,14 @@ void Fpga_Multicorrelator_8sc::fpga_configure_tracking_gps_local_code(int32_t PR { uint32_t k; - d_map_base[prog_mems_addr] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER; + d_map_base[prog_mems_addr] = local_code_fpga_clear_address_counter; for (k = 0; k < d_code_length_samples; k++) { d_map_base[prog_mems_addr] = d_ca_codes[(d_code_length_samples * (PRN - 1)) + k]; } if (d_track_pilot) { - d_map_base[prog_mems_addr] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER; + d_map_base[prog_mems_addr] = local_code_fpga_clear_address_counter; for (k = 0; k < d_code_length_samples; k++) { d_map_base[prog_mems_addr] = d_data_codes[(d_code_length_samples * (PRN - 1)) + k]; @@ -353,7 +339,7 @@ void Fpga_Multicorrelator_8sc::fpga_compute_code_shift_parameters(void) frac_part = frac_part + 1.0; // fmod operator does not work as in Matlab with negative numbers } - d_initial_interp_counter[i] = static_cast(floor(MAX_CODE_RESAMPLER_COUNTER * frac_part)); + d_initial_interp_counter[i] = static_cast(floor(max_code_resampler_counter * frac_part)); } if (d_track_pilot) { @@ -370,7 +356,7 @@ void Fpga_Multicorrelator_8sc::fpga_compute_code_shift_parameters(void) { frac_part = frac_part + 1.0; // fmod operator does not work as in Matlab with negative numbers } - d_initial_interp_counter[d_n_correlators] = static_cast(floor(MAX_CODE_RESAMPLER_COUNTER * frac_part)); + d_initial_interp_counter[d_n_correlators] = static_cast(floor(max_code_resampler_counter * frac_part)); } } @@ -394,8 +380,8 @@ void Fpga_Multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(void) { float d_rem_carrier_phase_in_rad_temp; - d_code_phase_step_chips_num = static_cast(roundf(MAX_CODE_RESAMPLER_COUNTER * d_code_phase_step_chips)); - d_code_phase_rate_step_chips_num = static_cast(roundf(MAX_CODE_RESAMPLER_COUNTER * d_code_phase_rate_step_chips)); + d_code_phase_step_chips_num = static_cast(roundf(max_code_resampler_counter * d_code_phase_step_chips)); + d_code_phase_rate_step_chips_num = static_cast(roundf(max_code_resampler_counter * d_code_phase_rate_step_chips)); if (d_rem_carrier_phase_in_rad > M_PI) { @@ -410,9 +396,9 @@ void Fpga_Multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(void) d_rem_carrier_phase_in_rad_temp = d_rem_carrier_phase_in_rad; } - d_rem_carr_phase_rad_int = static_cast(roundf((d_rem_carrier_phase_in_rad_temp)*PHASE_CARR_MAX_div_PI)); - d_phase_step_rad_int = static_cast(roundf((d_phase_step_rad)*PHASE_CARR_MAX_div_PI)); // the FPGA accepts a range for the phase step between -pi and +pi - d_carrier_phase_rate_step_rad_int = static_cast(roundf((d_carrier_phase_rate_step_rad)*PHASE_CARR_MAX_div_PI)); + d_rem_carr_phase_rad_int = static_cast(roundf((d_rem_carrier_phase_in_rad_temp)*PHASE_CARR_MAX_DIV_PI)); + d_phase_step_rad_int = static_cast(roundf((d_phase_step_rad)*PHASE_CARR_MAX_DIV_PI)); // the FPGA accepts a range for the phase step between -pi and +pi + d_carrier_phase_rate_step_rad_int = static_cast(roundf((d_carrier_phase_rate_step_rad)*PHASE_CARR_MAX_DIV_PI)); } @@ -479,7 +465,7 @@ void Fpga_Multicorrelator_8sc::unlock_channel(void) void Fpga_Multicorrelator_8sc::close_device() { auto *aux = const_cast(d_map_base); - if (munmap(static_cast(aux), PAGE_SIZE) == -1) + if (munmap(static_cast(aux), page_size) == -1) { std::cout << "Failed to unmap memory uio" << std::endl; } @@ -571,7 +557,6 @@ void Fpga_Multicorrelator_8sc::write_secondary_code(uint32_t secondary_code_leng write_val = write_val | (mem_addr * secondary_code_addr_bits) | (secondary_code_wr_strobe); d_map_base[reg_addr] = write_val; - } void Fpga_Multicorrelator_8sc::enable_secondary_codes() diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.h b/src/algorithms/tracking/libs/fpga_multicorrelator.h index 4712c7d92..a362d49cf 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.h +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.h @@ -40,8 +40,9 @@ #include #include - - +// floating point math constants related to the parameters that are written in the FPGA +#define PHASE_CARR_MAX_DIV_PI 683565275.5764316 // 2^(31)/pi +#define TWO_PI 6.283185307179586 /*! * \brief Class that implements carrier wipe-off and correlators. @@ -76,7 +77,6 @@ public: private: - // FPGA register addresses // write addresses @@ -107,14 +107,18 @@ private: static const uint32_t result_reg_imag_base_addr = 7; static const uint32_t sample_counter_reg_addr_lsw = 13; static const uint32_t sample_counter_reg_addr_msw = 14; - // FPGA-related constants static const uint32_t secondary_code_word_size = 20; // the secondary codes are written in to the FPGA in words of secondary_code_word_size bits - static const uint32_t secondary_code_wr_strobe = 0x800000; // write strobe position in the secondary code write register - static const uint32_t secondary_code_addr_bits = 0x100000; // memory address position in the secondary code write register - static const uint32_t drop_samples = 1; // bit 0 of drop_samples_reg_addr - static const uint32_t enable_secondary_code = 2; // bit 1 of drop_samples_reg_addr - static const uint32_t init_secondary_code_addresses = 4; // bit 2 of drop_samples_reg_addr + static const uint32_t secondary_code_wr_strobe = 0x800000; // write strobe position in the secondary code write register + static const uint32_t secondary_code_addr_bits = 0x100000; // memory address position in the secondary code write register + static const uint32_t drop_samples = 1; // bit 0 of drop_samples_reg_addr + static const uint32_t enable_secondary_code = 2; // bit 1 of drop_samples_reg_addr + static const uint32_t init_secondary_code_addresses = 4; // bit 2 of drop_samples_reg_addr + static const uint32_t page_size = 0x10000; + static const uint32_t max_length_deviceio_name = 50; + static const uint32_t max_code_resampler_counter = 1 << 20; // 2^(number of bits of precision of the code resampler) + static const uint32_t local_code_fpga_clear_address_counter = 0x10000000; + static const uint32_t test_register_track_writeval = 0x55AA; gr_complex *d_corr_out; gr_complex *d_Prompt_Data; diff --git a/src/core/libs/gnss_sdr_fpga_sample_counter.cc b/src/core/libs/gnss_sdr_fpga_sample_counter.cc index 85c6f6018..b20132a50 100644 --- a/src/core/libs/gnss_sdr_fpga_sample_counter.cc +++ b/src/core/libs/gnss_sdr_fpga_sample_counter.cc @@ -43,8 +43,6 @@ #include // for write, close, read, ssize_t -#define PAGE_SIZE 0x10000 // default page size for the multicorrelator memory map -#define TEST_REG_SANITY_CHECK 0x55AA // value to check the presence of the test register (to detect the hw) #ifndef TEMP_FAILURE_RETRY #define TEMP_FAILURE_RETRY(exp) \ ({ \ @@ -151,7 +149,7 @@ void gnss_sdr_fpga_sample_counter::open_device() LOG(WARNING) << "Cannot open deviceio" << device_name; std::cout << "Counter-Intr: cannot open deviceio" << device_name << std::endl; } - map_base = reinterpret_cast(mmap(nullptr, PAGE_SIZE, + map_base = reinterpret_cast(mmap(nullptr, page_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0)); if (map_base == reinterpret_cast(-1)) @@ -161,7 +159,7 @@ void gnss_sdr_fpga_sample_counter::open_device() } // sanity check : check test register - uint32_t writeval = TEST_REG_SANITY_CHECK; + uint32_t writeval = test_reg_sanity_check; uint32_t readval; readval = gnss_sdr_fpga_sample_counter::test_register(writeval); if (writeval != readval) @@ -181,7 +179,7 @@ void gnss_sdr_fpga_sample_counter::close_device() map_base[2] = 0; // disable the generation of the interrupt in the device auto *aux = const_cast(map_base); - if (munmap(static_cast(aux), PAGE_SIZE) == -1) + if (munmap(static_cast(aux), page_size) == -1) { std::cout << "Failed to unmap memory uio" << std::endl; } diff --git a/src/core/libs/gnss_sdr_fpga_sample_counter.h b/src/core/libs/gnss_sdr_fpga_sample_counter.h index 18b232869..d5b9b81d9 100644 --- a/src/core/libs/gnss_sdr_fpga_sample_counter.h +++ b/src/core/libs/gnss_sdr_fpga_sample_counter.h @@ -55,6 +55,9 @@ public: gr_vector_void_star &output_items); private: + static const uint32_t page_size = 0x10000; // default page size for the multicorrelator memory map + static const uint32_t test_reg_sanity_check = 0x55AA; // value to check the presence of the test register (to detect the hw) + friend gnss_sdr_fpga_sample_counter_sptr gnss_sdr_make_fpga_sample_counter(double _fs, int32_t _interval_ms); gnss_sdr_fpga_sample_counter(double _fs, int32_t _interval_ms); uint32_t test_register(uint32_t writeval); From f30d8a60ec5348995e74f412ab399d244ae912f0 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Tue, 23 Jul 2019 19:05:43 +0200 Subject: [PATCH 19/22] updated doxygen comments --- ...lileo_e1_pcps_ambiguous_acquisition_fpga.h | 34 +++++++- .../galileo_e5a_pcps_acquisition_fpga.h | 33 +++++++- .../gps_l1_ca_pcps_acquisition_fpga.h | 34 +++++++- .../adapters/gps_l5i_pcps_acquisition_fpga.h | 34 +++++++- .../gnuradio_blocks/pcps_acquisition_fpga.h | 4 +- .../acquisition/libs/fpga_acquisition.cc | 19 +---- .../acquisition/libs/fpga_acquisition.h | 40 +++++++-- .../signal_source/libs/fpga_switch.h | 16 ++++ .../galileo_e1_dll_pll_veml_tracking_fpga.h | 45 +++++++++-- .../galileo_e5a_dll_pll_tracking_fpga.h | 44 ++++++++-- .../gps_l1_ca_dll_pll_tracking_fpga.h | 41 ++++++++-- .../adapters/gps_l5_dll_pll_tracking_fpga.h | 44 ++++++++-- .../dll_pll_veml_tracking_fpga.cc | 12 +-- .../dll_pll_veml_tracking_fpga.h | 29 +++++++ .../tracking/libs/fpga_multicorrelator.cc | 6 +- .../tracking/libs/fpga_multicorrelator.h | 81 ++++++++++++++++++- 16 files changed, 450 insertions(+), 66 deletions(-) diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h index 1333619f6..40c8aed9c 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h @@ -50,13 +50,23 @@ class ConfigurationInterface; class GalileoE1PcpsAmbiguousAcquisitionFpga : public AcquisitionInterface { public: + + /*! + * \brief Constructor + */ GalileoE1PcpsAmbiguousAcquisitionFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ ~GalileoE1PcpsAmbiguousAcquisitionFpga() = default; + /*! + * \brief Role + */ inline std::string role() override { return role_; @@ -70,14 +80,32 @@ public: return "Galileo_E1_PCPS_Ambiguous_Acquisition_Fpga"; } + /*! + * \brief Returns size of lv_16sc_t + */ size_t item_size() override { - return item_size_; + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -150,6 +178,9 @@ public: */ void stop_acquisition() override; + /*! + * \brief Set resampler latency + */ void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: @@ -164,7 +195,6 @@ private: ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; - size_t item_size_; bool acquire_pilot_; uint32_t channel_; std::weak_ptr channel_fsm_; diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h index b12c3ea8f..6c624fbf8 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h @@ -50,13 +50,23 @@ class ConfigurationInterface; class GalileoE5aPcpsAcquisitionFpga : public AcquisitionInterface { public: + + /*! + * \brief Constructor + */ GalileoE5aPcpsAcquisitionFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ ~GalileoE5aPcpsAcquisitionFpga() = default; + /*! + * \brief Role + */ inline std::string role() override { return role_; @@ -70,14 +80,32 @@ public: return "Galileo_E5a_Pcps_Acquisition_Fpga"; } + /*! + * \brief Returns size of lv_16sc_t + */ inline size_t item_size() override { - return item_size_; + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -158,7 +186,7 @@ public: void stop_acquisition() override; /*! - * \brief Sets the resampler latency to account it in the acquisition code delay estimation + * \brief Set resampler latency */ void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; @@ -174,7 +202,6 @@ private: ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; - size_t item_size_; std::string item_type_; std::string dump_filename_; std::string role_; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h index b38f123df..3cf09028b 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h @@ -52,13 +52,23 @@ class ConfigurationInterface; class GpsL1CaPcpsAcquisitionFpga : public AcquisitionInterface { public: + + /*! + * \brief Constructor + */ GpsL1CaPcpsAcquisitionFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ ~GpsL1CaPcpsAcquisitionFpga() = default; + /*! + * \brief Role + */ inline std::string role() override { return role_; @@ -72,14 +82,32 @@ public: return "GPS_L1_CA_PCPS_Acquisition_Fpga"; } + /*! + * \brief Returns size of lv_16sc_t + */ inline size_t item_size() override { - return item_size_; + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -152,6 +180,9 @@ public: */ void stop_acquisition() override; + /*! + * \brief Set Resampler Latency + */ void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: @@ -168,7 +199,6 @@ private: ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; - size_t item_size_; uint32_t channel_; std::weak_ptr channel_fsm_; uint32_t doppler_max_; diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h index f4d7bdcb7..67c93c2d6 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h @@ -50,13 +50,23 @@ class ConfigurationInterface; class GpsL5iPcpsAcquisitionFpga : public AcquisitionInterface { public: + + /*! + * \brief Constructor + */ GpsL5iPcpsAcquisitionFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ ~GpsL5iPcpsAcquisitionFpga() = default; + /*! + * \brief Role + */ inline std::string role() override { return role_; @@ -70,14 +80,32 @@ public: return "GPS_L5i_PCPS_Acquisition_Fpga"; } + /*! + * \brief Returns size of lv_16sc_t + */ inline size_t item_size() override { - return item_size_; + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -150,6 +178,9 @@ public: */ void stop_acquisition() override; + /*! + * \brief Set resampler latency + */ void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: @@ -165,7 +196,6 @@ private: ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; - size_t item_size_; std::string item_type_; uint32_t channel_; std::weak_ptr channel_fsm_; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h index b2933c7f5..d1e617025 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h @@ -89,6 +89,9 @@ pcps_acquisition_fpga_sptr pcps_make_acquisition_fpga(pcpsconf_fpga_t conf_); class pcps_acquisition_fpga { public: + /*! + * \brief Destructor + */ ~pcps_acquisition_fpga() = default; /*! @@ -116,7 +119,6 @@ public: /*! * \brief Sets local code for PCPS acquisition algorithm. - * \param code - Pointer to the PRN code. */ void set_local_code(); diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.cc b/src/algorithms/acquisition/libs/fpga_acquisition.cc index 7168113ef..bd1fa31e4 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.cc +++ b/src/algorithms/acquisition/libs/fpga_acquisition.cc @@ -176,11 +176,6 @@ void Fpga_Acquisition::run_acquisition(void) std::cout << "acquisition module Interrupt number " << irq_count << std::endl; } - // nbytes = TEMP_FAILURE_RETRY(write(d_fd, reinterpret_cast(&disable_int), sizeof(int32_t))); - // if (nbytes != sizeof(int32_t)) - // { - // std::cerr << "Error disabling interruptions in the FPGA." << std::endl; - // } } @@ -213,7 +208,7 @@ void Fpga_Acquisition::set_doppler_sweep(uint32_t num_sweeps, uint32_t doppler_s void Fpga_Acquisition::configure_acquisition() { - //Fpga_Acquisition::open_device(); + //Fpga_Acquisition::(); d_map_base[0] = d_select_queue; d_map_base[1] = d_vector_length; d_map_base[2] = d_nsamples; @@ -258,18 +253,6 @@ void Fpga_Acquisition::read_acquisition_results(uint32_t *max_index, } -void Fpga_Acquisition::block_samples() -{ - d_map_base[14] = 1; // block the samples -} - - -void Fpga_Acquisition::unblock_samples() -{ - d_map_base[14] = 0; // unblock the samples -} - - void Fpga_Acquisition::close_device() { auto *aux = const_cast(d_map_base); diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.h b/src/algorithms/acquisition/libs/fpga_acquisition.h index 2a85aa7ab..188cddfe4 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.h +++ b/src/algorithms/acquisition/libs/fpga_acquisition.h @@ -45,6 +45,10 @@ class Fpga_Acquisition { public: + + /*! + * \brief Constructor + */ Fpga_Acquisition( std::string device_name, uint32_t nsamples, @@ -56,14 +60,29 @@ public: uint32_t *all_fft_codes, uint32_t excludelimit); + /*! + * \brief Destructor + */ ~Fpga_Acquisition() = default; + /*! + * \brief Select the code with the chosen PRN + */ bool set_local_code(uint32_t PRN); + /*! + * \brief Configure the doppler sweep parameters in the FPGA + */ void set_doppler_sweep(uint32_t num_sweeps, uint32_t doppler_step, int32_t doppler_min); + /*! + * \brief Run the acquisition process in the FPGA + */ void run_acquisition(void); + /*! + * \brief Read the results of the acquisition process + */ void read_acquisition_results( uint32_t *max_index, float *firstpeak, @@ -73,10 +92,6 @@ public: uint32_t *doppler_index, uint32_t *total_blk_exp); - void block_samples(); - - void unblock_samples(); - /*! * \brief Set maximum Doppler grid search * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. @@ -101,18 +116,33 @@ public: void reset_acquisition(void); /*! - * \brief read the scaling factor that has been used by the FFT-IFFT + * \brief Read the scaling factor that has been used by the FFT-IFFT */ void read_fpga_total_scale_factor(uint32_t *total_scale_factor, uint32_t *fw_scale_factor); + /*! + * \brief Set the block exponent of the FFT in the FPGA. + */ void set_block_exp(uint32_t total_block_exp); + /*! + * \brief Write the PRN code in the FPGA + */ void write_local_code(void); + /*! + * \brief Write the acquisition parameters into the FPGA + */ void configure_acquisition(void); + /*! + * \brief Open the device driver + */ void open_device(); + /*! + * \brief Close the device driver + */ void close_device(); private: diff --git a/src/algorithms/signal_source/libs/fpga_switch.h b/src/algorithms/signal_source/libs/fpga_switch.h index 51d198fa3..551adc521 100644 --- a/src/algorithms/signal_source/libs/fpga_switch.h +++ b/src/algorithms/signal_source/libs/fpga_switch.h @@ -39,11 +39,27 @@ #include +/*! + * \brief Class that controls the switch in the FPGA, which connects the FPGA acquisition and multicorrelator modules to + * either the DMA or the Analog Front-End. + */ class Fpga_Switch { public: + + /*! + * \brief Constructor + */ Fpga_Switch(const std::string& device_name); + + /*! + * \brief Destructor + */ ~Fpga_Switch(); + + /*! + * \brief This function configures the switch in th eFPGA + */ void set_switch_position(int32_t switch_position); private: diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h index 28c732837..60800a595 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h @@ -51,32 +51,62 @@ class ConfigurationInterface; class GalileoE1DllPllVemlTrackingFpga : public TrackingInterface { public: + + /*! + * \brief Constructor + */ GalileoE1DllPllVemlTrackingFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ virtual ~GalileoE1DllPllVemlTrackingFpga(); + /*! + * \brief Role + */ inline std::string role() override { return role_; } - //! Returns "Galileo_E1_DLL_PLL_VEML_Tracking_Fpga" + /*! + * \brief Returns "Galileo_E1_DLL_PLL_VEML_Tracking_Fpga" + */ inline std::string implementation() override { return "Galileo_E1_DLL_PLL_VEML_Tracking_Fpga"; } - inline size_t item_size() override + /*! + * \brief Returns size of lv_16sc_t + */ + size_t item_size() override { - return item_size_; + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -91,9 +121,13 @@ public: */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; - void start_tracking() override; /*! - * \brief Stop running tracking + * \brief Start the tracking process in the FPGA + */ + void start_tracking() override; + + /*! + * \brief Stop the tracking process in the FPGA */ void stop_tracking() override; @@ -105,7 +139,6 @@ private: static const int32_t LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT = 0x20000000; // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; - size_t item_size_; uint32_t channel_; std::string role_; uint32_t in_streams_; diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h index 0a489478d..cfb1afc7e 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h @@ -44,32 +44,61 @@ class ConfigurationInterface; class GalileoE5aDllPllTrackingFpga : public TrackingInterface { public: + /*! + * \brief Constructor + */ GalileoE5aDllPllTrackingFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ virtual ~GalileoE5aDllPllTrackingFpga(); + /*! + * \brief Role + */ inline std::string role() override { return role_; } - //! Returns "Galileo_E5a_DLL_PLL_Tracking_Fpga" + /*! + * \brief Returns "Galileo_E5a_DLL_PLL_Tracking_Fpga" + */ inline std::string implementation() override { return "Galileo_E5a_DLL_PLL_Tracking_Fpga"; } - inline size_t item_size() override + /*! + * \brief Returns size of lv_16sc_t + */ + size_t item_size() override { - return item_size_; + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -83,9 +112,13 @@ public: */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; - void start_tracking() override; /*! - * \brief Stop running tracking + * \brief Start the tracking process in the FPGA + */ + void start_tracking() override; + + /*! + * \brief Stop the tracking process in the FPGA */ void stop_tracking() override; @@ -98,7 +131,6 @@ private: dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; - size_t item_size_; uint32_t channel_; std::string role_; uint32_t in_streams_; diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h index a77c2d4d0..ac49be6c9 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h @@ -49,32 +49,61 @@ class ConfigurationInterface; class GpsL1CaDllPllTrackingFpga : public TrackingInterface { public: + /*! + * \brief Constructor + */ GpsL1CaDllPllTrackingFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ virtual ~GpsL1CaDllPllTrackingFpga(); + /*! + * \brief Role + */ inline std::string role() override { return role_; } - //! Returns "GPS_L1_CA_DLL_PLL_Tracking_Fpga" + /*! + * \brief Returns "GPS_L1_CA_DLL_PLL_Tracking_Fpga" + */ inline std::string implementation() override { return "GPS_L1_CA_DLL_PLL_Tracking_Fpga"; } - inline size_t item_size() override + /*! + * \brief Returns size of lv_16sc_t + */ + size_t item_size() override { - return item_size_; + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -88,10 +117,13 @@ public: */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; + /*! + * \brief Start the tracking process in the FPGA + */ void start_tracking() override; /*! - * \brief Stop running tracking + * \brief Stop the tracking process in the FPGA */ void stop_tracking() override; @@ -105,7 +137,6 @@ private: dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; - size_t item_size_; uint32_t channel_; std::string role_; uint32_t in_streams_; diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h index af3a58802..b3bef53ce 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.h @@ -50,32 +50,61 @@ class ConfigurationInterface; class GpsL5DllPllTrackingFpga : public TrackingInterface { public: + /*! + * \brief Constructor + */ GpsL5DllPllTrackingFpga(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); + /*! + * \brief Destructor + */ virtual ~GpsL5DllPllTrackingFpga(); + /*! + * \brief Role + */ inline std::string role() override { return role_; } - //! Returns "GPS_L5_DLL_PLL_Tracking_Fpga" + /*! + * \brief Returns "GPS_L5_DLL_PLL_Tracking_Fpga" + */ inline std::string implementation() override { return "GPS_L5_DLL_PLL_Tracking_Fpga"; } - inline size_t item_size() override + /*! + * \brief Returns size of lv_16sc_t + */ + size_t item_size() override { - return item_size_; + return sizeof(int16_t); } + /*! + * \brief Connect + */ void connect(gr::top_block_sptr top_block) override; + + /*! + * \brief Disconnect + */ void disconnect(gr::top_block_sptr top_block) override; + + /*! + * \brief Get left block + */ gr::basic_block_sptr get_left_block() override; + + /*! + * \brief Get right block + */ gr::basic_block_sptr get_right_block() override; /*! @@ -89,9 +118,13 @@ public: */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; - void start_tracking() override; /*! - * \brief Stop running tracking + * \brief Start the tracking process in the FPGA + */ + void start_tracking() override; + + /*! + * \brief Stop the tracking process in the FPGA */ void stop_tracking() override; @@ -104,7 +137,6 @@ private: static const int32_t LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT = 0x20000000; // flag that selects the writing of the pilot code in the FPGA (as opposed to the data code) dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; - size_t item_size_; uint32_t channel_; std::string role_; uint32_t in_streams_; 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 9e4918736..2344f54c6 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 @@ -1851,10 +1851,10 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // this must be computed for the secondary prn code if (d_secondary) { - uint32_t first_length_secondary_code = d_current_integration_length_samples - (d_fpga_integration_period - 1) * static_cast(std::floor(T_prn_samples)); - uint32_t next_length_secondary_code = static_cast(std::floor(T_prn_samples)); + uint32_t first_prn_length = d_current_integration_length_samples - (d_fpga_integration_period - 1) * static_cast(std::floor(T_prn_samples)); + uint32_t next_prn_length = static_cast(std::floor(T_prn_samples)); - multicorrelator_fpga->update_secondary_code_length(first_length_secondary_code, next_length_secondary_code); + multicorrelator_fpga->update_prn_code_length(first_prn_length, next_prn_length); } // perform a correlation step @@ -1898,10 +1898,10 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // this must be computed for the secondary prn code if (d_secondary) { - uint32_t first_length_secondary_code = d_current_integration_length_samples - (d_fpga_integration_period - 1) * static_cast(std::floor(T_prn_samples)); - uint32_t next_length_secondary_code = static_cast(std::floor(T_prn_samples)); + uint32_t first_prn_length = d_current_integration_length_samples - (d_fpga_integration_period - 1) * static_cast(std::floor(T_prn_samples)); + uint32_t next_prn_length = static_cast(std::floor(T_prn_samples)); - multicorrelator_fpga->update_secondary_code_length(first_length_secondary_code, next_length_secondary_code); + multicorrelator_fpga->update_prn_code_length(first_prn_length, next_prn_length); } // perform a correlation step diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index ff55c5a30..53e6aad84 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -62,16 +62,45 @@ dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Co class dll_pll_veml_tracking_fpga : public gr::block { public: + /*! + * \brief Destructor + */ ~dll_pll_veml_tracking_fpga(); + /*! + * \brief Set the channel number and configure some multicorrelator parameters + */ void set_channel(uint32_t channel); + + /*! + * \brief This function is used with two purposes: + * 1 -> To set the gnss_synchro + * 2 -> A set_gnss_synchro command with a valid PRN is received when the system is going to run + * acquisition with that PRN. We can use this command to pre-initialize tracking parameters and + * variables before the actual acquisition process takes place. In this way we minimize the + * latency between acquisition and tracking once the acquisition has been made. + */ void set_gnss_synchro(Gnss_Synchro *p_gnss_synchro); + + /*! + * \brief This function starts the tracking process + */ void start_tracking(); + + /*! + * \brief This function sets a flag that makes general_work to stop in order to finish the tracking process. + */ void stop_tracking(); + /*! + * \brief General Work + */ int general_work(int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + /*! + * \brief This function disables the HW multicorrelator in the FPGA in order to stop the tracking process + */ void reset(void); private: diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.cc b/src/algorithms/tracking/libs/fpga_multicorrelator.cc index 66d569f0a..4f0b1f06f 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.cc +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.cc @@ -491,10 +491,10 @@ void Fpga_Multicorrelator_8sc::set_secondary_code_lengths(uint32_t secondary_cod d_map_base[secondary_code_lengths_reg_addr] = secondary_code_length_1_minus_1 * 256 + secondary_code_length_0_minus_1; } -void Fpga_Multicorrelator_8sc::update_secondary_code_length(uint32_t first_length_secondary_code, uint32_t next_length_secondary_code) +void Fpga_Multicorrelator_8sc::update_prn_code_length(uint32_t first_prn_length, uint32_t next_prn_length) { - d_map_base[first_prn_length_minus_1_reg_addr] = first_length_secondary_code - 1; - d_map_base[next_prn_length_minus_1_reg_addr] = next_length_secondary_code - 1; + d_map_base[first_prn_length_minus_1_reg_addr] = first_prn_length - 1; + d_map_base[next_prn_length_minus_1_reg_addr] = next_prn_length - 1; } void Fpga_Multicorrelator_8sc::initialize_secondary_code(uint32_t secondary_code, std::string *secondary_code_string) diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.h b/src/algorithms/tracking/libs/fpga_multicorrelator.h index a362d49cf..b2da1b53a 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.h +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.h @@ -50,29 +50,108 @@ class Fpga_Multicorrelator_8sc { public: + + /*! + * \brief Constructor + */ Fpga_Multicorrelator_8sc(int32_t n_correlators, std::string device_name, uint32_t device_base, int32_t *ca_codes, int32_t *data_codes, uint32_t code_length_chips, bool track_pilot, uint32_t code_samples_per_chip); + + /*! + * \brief Destructor + */ ~Fpga_Multicorrelator_8sc(); + + /*! + * \brief Configure pointers to the FPGA multicorrelator results + */ void set_output_vectors(gr_complex *corr_out, gr_complex *Prompt_Data); + + /*! + * \brief Configure the local code in the FPGA multicorrelator + */ void set_local_code_and_taps( float *shifts_chips, float *prompt_data_shift, int32_t PRN); + + /*! + * \brief Configure code phase and code rate parameters in the FPGA + */ void update_local_code(); + + /*! + * \brief Perform a multicorrelation + */ void Carrier_wipeoff_multicorrelator_resampler( float rem_carrier_phase_in_rad, float phase_step_rad, float carrier_phase_rate_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int32_t signal_length_samples); + + /*! + * \brief Stop the correlation process in the FPGA and free code phase and code rate parameters + */ bool free(); + + /*! + * \brief Set channel number and open the FPGA device driver + */ void set_channel(uint32_t channel); + + /*! + * \brief Set the initial sample number where the tracking process begins + */ void set_initial_sample(uint64_t samples_offset); + + /*! + * \brief Read the sample counter in the FPGA + */ uint64_t read_sample_counter(); + + /*! + * \brief Start the tracking process in the FPGA + */ void lock_channel(void); + + /*! + * \brief finish the tracking process in the FPGA + */ void unlock_channel(void); + + /*! + * \brief Set the secondary code length in the FPGA. This is only used when extended coherent integration + * is enabled in the FPGA. If tracking the pilot is enabled then secondary_code_0_length is the length of the pilot + * secondary code and secondary_code_1_length is the length of the data secondary code. If tracking the pilot is disabled + * then secondary_code_0_length is the length of the data secondary code, and secondary_code_1_length must be set to zero. + */ void set_secondary_code_lengths(uint32_t secondary_code_0_length, uint32_t secondary_code_1_length); + + /*! + * \brief Initialize the secondary code in the FPGA. If tracking the pilot is enabled then the pilot secondary code is + * configured when secondary_code = 0 and the data secondary code is configured when secondary_code = 1. If tracking the + * pilot is disabled then the data secondary code is configured when secondary code = 0. + */ void initialize_secondary_code(uint32_t secondary_code, std::string *secondary_code_string); - void update_secondary_code_length(uint32_t first_length_secondary_code, uint32_t next_length_secondary_code); + + /*! + * \brief Set the PRN length in the FPGA in number of samples. This function is only used then extended coherent integration is enabled in the + * FPGA. The FPGA allows for the configuration of two PRN lengths. When the length of the extended coherent integration is bigger than the + * length of the PRN code, the FPGA uses the first_length_secondary_code as the length of the PRN code immediately following the beginning + * of the extended coherent integration, and the next_length_secondary_code as the length of the remaining PRN codes. + * The purpose of this is to have the option to allow the FPGA to compensate for a possible deviation between the nominal value of the PRN + * code length and the measured PRN code length in the PRN immediately following the start of the coherent integration only. + * If this option is not used then write the same value to first_length_secondary_code and next_length_secondary_code. + */ + void update_prn_code_length(uint32_t first_length_secondary_code, uint32_t next_length_secondary_code); + + /*! + * \brief Enable the use of secondary codes in the FPGA + */ void enable_secondary_codes(); + + /*! + * \brief Disable the use of secondary codes in the FPGA + */ void disable_secondary_codes(); From 48fe6086e496edac655a2b61397c88ebf489f884 Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Wed, 24 Jul 2019 11:26:16 +0200 Subject: [PATCH 20/22] updated the handling of the scoped_lock mutex in the fpga tracking loops + used clang-format. --- .../galileo_e1_pcps_ambiguous_acquisition_fpga.h | 8 +++----- .../adapters/galileo_e5a_pcps_acquisition_fpga.h | 10 ++++------ .../adapters/gps_l1_ca_pcps_acquisition_fpga.h | 1 - .../adapters/gps_l5i_pcps_acquisition_fpga.h | 1 - src/algorithms/acquisition/libs/fpga_acquisition.cc | 1 - src/algorithms/acquisition/libs/fpga_acquisition.h | 3 +-- src/algorithms/signal_source/libs/fpga_switch.h | 3 +-- .../adapters/galileo_e1_dll_pll_veml_tracking_fpga.h | 2 -- .../adapters/galileo_e5a_dll_pll_tracking_fpga.h | 1 - .../adapters/gps_l1_ca_dll_pll_tracking_fpga.h | 11 +++++------ .../gnuradio_blocks/dll_pll_veml_tracking_fpga.cc | 6 +++++- .../gnuradio_blocks/dll_pll_veml_tracking_fpga.h | 12 ++++++------ src/algorithms/tracking/libs/fpga_multicorrelator.h | 3 +-- .../gps_l1_ca_pcps_acquisition_test_fpga.cc | 2 +- 14 files changed, 27 insertions(+), 37 deletions(-) diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h index 40c8aed9c..c18bb9b1c 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h @@ -50,7 +50,6 @@ class ConfigurationInterface; class GalileoE1PcpsAmbiguousAcquisitionFpga : public AcquisitionInterface { public: - /*! * \brief Constructor */ @@ -184,14 +183,13 @@ public: void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: - // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. static const uint32_t quant_bits_local_code = 16; static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word - static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word - static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word - static const uint32_t shl_code_bits = 65536; // shift left by 10 bits + static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t shl_code_bits = 65536; // shift left by 10 bits ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h index 6c624fbf8..5cbb7c6a4 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h @@ -50,7 +50,6 @@ class ConfigurationInterface; class GalileoE5aPcpsAcquisitionFpga : public AcquisitionInterface { public: - /*! * \brief Constructor */ @@ -191,14 +190,13 @@ public: void set_resampler_latency(uint32_t latency_samples __attribute__((unused))) override{}; private: - // the following flags are FPGA-specific and they are using arrange the values of the fft of the local code in the way the FPGA // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. static const uint32_t quant_bits_local_code = 16; - static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word - static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word - static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word - static const uint32_t shl_code_bits = 65536; // shift left by 10 bits + static const uint32_t select_lsbits = 0x0000FFFF; // Select the 10 LSbits out of a 20-bit word + static const uint32_t select_msbits = 0xFFFF0000; // Select the 10 MSbits out of a 20-bit word + static const uint32_t select_all_code_bits = 0xFFFFFFFF; // Select a 20 bit word + static const uint32_t shl_code_bits = 65536; // shift left by 10 bits ConfigurationInterface* configuration_; pcps_acquisition_fpga_sptr acquisition_fpga_; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h index 3cf09028b..ceb7939dd 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h @@ -52,7 +52,6 @@ class ConfigurationInterface; class GpsL1CaPcpsAcquisitionFpga : public AcquisitionInterface { public: - /*! * \brief Constructor */ diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h index 67c93c2d6..4c2a5e726 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h @@ -50,7 +50,6 @@ class ConfigurationInterface; class GpsL5iPcpsAcquisitionFpga : public AcquisitionInterface { public: - /*! * \brief Constructor */ diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.cc b/src/algorithms/acquisition/libs/fpga_acquisition.cc index bd1fa31e4..94bd423f3 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.cc +++ b/src/algorithms/acquisition/libs/fpga_acquisition.cc @@ -175,7 +175,6 @@ void Fpga_Acquisition::run_acquisition(void) std::cout << "acquisition module Read failed to retrieve 4 bytes!" << std::endl; std::cout << "acquisition module Interrupt number " << irq_count << std::endl; } - } diff --git a/src/algorithms/acquisition/libs/fpga_acquisition.h b/src/algorithms/acquisition/libs/fpga_acquisition.h index 188cddfe4..1c49c23b1 100644 --- a/src/algorithms/acquisition/libs/fpga_acquisition.h +++ b/src/algorithms/acquisition/libs/fpga_acquisition.h @@ -45,8 +45,7 @@ class Fpga_Acquisition { public: - - /*! + /*! * \brief Constructor */ Fpga_Acquisition( diff --git a/src/algorithms/signal_source/libs/fpga_switch.h b/src/algorithms/signal_source/libs/fpga_switch.h index 551adc521..0ab1d2461 100644 --- a/src/algorithms/signal_source/libs/fpga_switch.h +++ b/src/algorithms/signal_source/libs/fpga_switch.h @@ -46,8 +46,7 @@ class Fpga_Switch { public: - - /*! + /*! * \brief Constructor */ Fpga_Switch(const std::string& device_name); diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h index 60800a595..b7e7cd721 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.h @@ -51,7 +51,6 @@ class ConfigurationInterface; class GalileoE1DllPllVemlTrackingFpga : public TrackingInterface { public: - /*! * \brief Constructor */ @@ -132,7 +131,6 @@ public: void stop_tracking() override; private: - // the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. static const int32_t LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY = 0x0C000000; // flag that enables WE (Write Enable) of the local code FPGA diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h index cfb1afc7e..ec574d412 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.h @@ -123,7 +123,6 @@ public: void stop_tracking() override; private: - // the following flags are FPGA-specific and they are using arrange the values of the local code in the way the FPGA // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. static const int32_t LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY = 0x0C000000; // flag that enables WE (Write Enable) of the local code FPGA diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h index ac49be6c9..ba08f0b76 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.h @@ -128,12 +128,11 @@ public: void stop_tracking() override; private: - - static const uint32_t NUM_PRNs = 32; // total number of PRNs - static const int32_t GPS_CA_BIT_DURATION_MS = 20; - // the following flag is FPGA-specific and they are using arrange the values of the local code in the way the FPGA - // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. - static const int32_t LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY = 0x0C000000; // flag that enables WE (Write Enable) of the local code FPGA + static const uint32_t NUM_PRNs = 32; // total number of PRNs + static const int32_t GPS_CA_BIT_DURATION_MS = 20; + // the following flag is FPGA-specific and they are using arrange the values of the local code in the way the FPGA + // expects. This arrangement is done in the initialisation to avoid consuming unnecessary clock cycles during tracking. + static const int32_t LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY = 0x0C000000; // flag that enables WE (Write Enable) of the local code FPGA dll_pll_veml_tracking_fpga_sptr tracking_fpga_sc; 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 2344f54c6..150ab3f27 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 @@ -1286,6 +1286,8 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile() void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel) { + gr::thread::scoped_lock l(d_setlock); + d_channel = channel; multicorrelator_fpga->set_channel(d_channel); LOG(INFO) << "Tracking Channel set to " << d_channel; @@ -1458,19 +1460,20 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) d_cn0_smoother.reset(); d_carrier_lock_test_smoother.reset(); - } } void dll_pll_veml_tracking_fpga::stop_tracking() { + // interrupt the tracking loops d_stop_tracking = true; } void dll_pll_veml_tracking_fpga::reset(void) { + gr::thread::scoped_lock l(d_setlock); multicorrelator_fpga->unlock_channel(); } @@ -1480,6 +1483,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un gr_vector_const_void_star &input_items __attribute__((unused)), gr_vector_void_star &output_items) { + gr::thread::scoped_lock l(d_setlock); auto **out = reinterpret_cast(&output_items[0]); Gnss_Synchro current_synchro_data = Gnss_Synchro(); current_synchro_data.Flag_valid_symbol_output = false; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 53e6aad84..6602cfc8c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -62,12 +62,12 @@ dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Co class dll_pll_veml_tracking_fpga : public gr::block { public: - /*! + /*! * \brief Destructor */ ~dll_pll_veml_tracking_fpga(); - /*! + /*! * \brief Set the channel number and configure some multicorrelator parameters */ void set_channel(uint32_t channel); @@ -82,23 +82,23 @@ public: */ void set_gnss_synchro(Gnss_Synchro *p_gnss_synchro); - /*! + /*! * \brief This function starts the tracking process */ void start_tracking(); - /*! + /*! * \brief This function sets a flag that makes general_work to stop in order to finish the tracking process. */ void stop_tracking(); - /*! + /*! * \brief General Work */ int general_work(int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - /*! + /*! * \brief This function disables the HW multicorrelator in the FPGA in order to stop the tracking process */ void reset(void); diff --git a/src/algorithms/tracking/libs/fpga_multicorrelator.h b/src/algorithms/tracking/libs/fpga_multicorrelator.h index b2da1b53a..fdefc628f 100644 --- a/src/algorithms/tracking/libs/fpga_multicorrelator.h +++ b/src/algorithms/tracking/libs/fpga_multicorrelator.h @@ -50,8 +50,7 @@ class Fpga_Multicorrelator_8sc { public: - - /*! + /*! * \brief Constructor */ Fpga_Multicorrelator_8sc(int32_t n_correlators, std::string device_name, diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test_fpga.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test_fpga.cc index dcc07a9e5..3edd0ebf8 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test_fpga.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test_fpga.cc @@ -29,6 +29,7 @@ * ------------------------------------------------------------------------- */ +#include "concurrent_queue.h" #include "gnss_block_factory.h" #include "gnss_block_interface.h" #include "gnss_sdr_valve.h" @@ -41,7 +42,6 @@ #include #include #include -#include "concurrent_queue.h" #include #include #include From 7f594a356bc7fa1289ba39848f434bcc54c64e4b Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Wed, 24 Jul 2019 13:02:14 +0200 Subject: [PATCH 21/22] doppler center frequency for the grid search --- ...ileo_e1_pcps_ambiguous_acquisition_fpga.cc | 6 +++++ ...lileo_e1_pcps_ambiguous_acquisition_fpga.h | 6 +++++ .../galileo_e5a_pcps_acquisition_fpga.cc | 6 +++++ .../galileo_e5a_pcps_acquisition_fpga.h | 6 +++++ .../gps_l1_ca_pcps_acquisition_fpga.cc | 6 +++++ .../gps_l1_ca_pcps_acquisition_fpga.h | 6 +++++ .../adapters/gps_l5i_pcps_acquisition_fpga.cc | 6 +++++ .../adapters/gps_l5i_pcps_acquisition_fpga.h | 6 +++++ .../gnuradio_blocks/pcps_acquisition_fpga.cc | 24 ++++--------------- .../gnuradio_blocks/pcps_acquisition_fpga.h | 16 ++++++++++++- 10 files changed, 68 insertions(+), 20 deletions(-) diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc index ca936e131..7621bd00c 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc @@ -213,6 +213,12 @@ void GalileoE1PcpsAmbiguousAcquisitionFpga::set_doppler_step(unsigned int dopple acquisition_fpga_->set_doppler_step(doppler_step_); } +void GalileoE1PcpsAmbiguousAcquisitionFpga::set_doppler_center(int doppler_center) +{ + doppler_center_ = doppler_center; + + acquisition_fpga_->set_doppler_center(doppler_center_); +} void GalileoE1PcpsAmbiguousAcquisitionFpga::set_gnss_synchro(Gnss_Synchro* gnss_synchro) { diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h index c18bb9b1c..97bea5acc 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.h @@ -147,6 +147,11 @@ public: */ void set_doppler_step(unsigned int doppler_step) override; + /*! + * \brief Set Doppler center for the grid search + */ + void set_doppler_center(int doppler_center) override; + /*! * \brief Initializes acquisition algorithm. */ @@ -198,6 +203,7 @@ private: std::weak_ptr channel_fsm_; uint32_t doppler_max_; uint32_t doppler_step_; + int32_t doppler_center_; std::string dump_filename_; Gnss_Synchro* gnss_synchro_; std::string role_; diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc index d1df3eca1..a2b0194d1 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc @@ -216,6 +216,12 @@ void GalileoE5aPcpsAcquisitionFpga::set_doppler_step(unsigned int doppler_step) acquisition_fpga_->set_doppler_step(doppler_step_); } +void GalileoE5aPcpsAcquisitionFpga::set_doppler_center(int doppler_center) +{ + doppler_center_ = doppler_center; + + acquisition_fpga_->set_doppler_center(doppler_center_); +} void GalileoE5aPcpsAcquisitionFpga::set_gnss_synchro(Gnss_Synchro* gnss_synchro) { diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h index 5cbb7c6a4..5326df685 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.h @@ -147,6 +147,11 @@ public: */ void set_doppler_step(unsigned int doppler_step) override; + /*! + * \brief Set Doppler center for the grid search + */ + void set_doppler_center(int doppler_center) override; + /*! * \brief Initializes acquisition algorithm. */ @@ -209,6 +214,7 @@ private: std::weak_ptr channel_fsm_; uint32_t doppler_max_; uint32_t doppler_step_; + int32_t doppler_center_; unsigned int in_streams_; unsigned int out_streams_; Gnss_Synchro* gnss_synchro_; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc index f66c4a2c7..d82de9ba8 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc @@ -192,6 +192,12 @@ void GpsL1CaPcpsAcquisitionFpga::set_doppler_step(unsigned int doppler_step) acquisition_fpga_->set_doppler_step(doppler_step_); } +void GpsL1CaPcpsAcquisitionFpga::set_doppler_center(int doppler_center) +{ + doppler_center_ = doppler_center; + + acquisition_fpga_->set_doppler_center(doppler_center_); +} void GpsL1CaPcpsAcquisitionFpga::set_gnss_synchro(Gnss_Synchro* gnss_synchro) { diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h index ceb7939dd..05256163f 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h @@ -149,6 +149,11 @@ public: */ void set_doppler_step(unsigned int doppler_step) override; + /*! + * \brief Set Doppler center for the grid search + */ + void set_doppler_center(int doppler_center) override; + /*! * \brief Initializes acquisition algorithm. */ @@ -202,6 +207,7 @@ private: std::weak_ptr channel_fsm_; uint32_t doppler_max_; uint32_t doppler_step_; + int32_t doppler_center_; Gnss_Synchro* gnss_synchro_; std::string role_; unsigned int in_streams_; diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc index 3ecd513a5..6ecd55340 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc @@ -196,6 +196,12 @@ void GpsL5iPcpsAcquisitionFpga::set_doppler_step(unsigned int doppler_step) acquisition_fpga_->set_doppler_step(doppler_step_); } +void GpsL5iPcpsAcquisitionFpga::set_doppler_center(int doppler_center) +{ + doppler_center_ = doppler_center; + + acquisition_fpga_->set_doppler_center(doppler_center_); +} void GpsL5iPcpsAcquisitionFpga::set_gnss_synchro(Gnss_Synchro* gnss_synchro) { diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h index 4c2a5e726..4cec033d1 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.h @@ -147,6 +147,11 @@ public: */ void set_doppler_step(unsigned int doppler_step) override; + /*! + * \brief Set Doppler center for the grid search + */ + void set_doppler_center(int doppler_center) override; + /*! * \brief Initializes acquisition algorithm. */ @@ -200,6 +205,7 @@ private: std::weak_ptr channel_fsm_; uint32_t doppler_max_; uint32_t doppler_step_; + int32_t doppler_center_; std::string dump_filename_; Gnss_Synchro* gnss_synchro_; std::string role_; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc index 521b5e767..b15be13a7 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc @@ -58,6 +58,7 @@ pcps_acquisition_fpga::pcps_acquisition_fpga(pcpsconf_fpga_t conf_) d_num_doppler_bins = 0U; d_threshold = 0.0; d_doppler_step = 0U; + d_doppler_center = 0U; d_doppler_index = 0U; d_test_statistics = 0.0; d_channel = 0U; @@ -139,25 +140,10 @@ void pcps_acquisition_fpga::send_positive_acquisition() << ", code phase " << d_gnss_synchro->Acq_delay_samples << ", doppler " << d_gnss_synchro->Acq_doppler_hz << ", magnitude " << d_mag - << ", input signal power " << d_input_power; + << ", input signal power " << d_input_power + << ", Assist doppler_center " << d_doppler_center; - // std::cout << "positive acquisition" - // << ", satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN - // << ", sample_stamp " << d_sample_counter - // << ", test statistics value " << d_test_statistics - // << ", test statistics threshold " << d_threshold - // << ", code phase " << d_gnss_synchro->Acq_delay_samples - // << ", doppler " << d_gnss_synchro->Acq_doppler_hz - // << ", magnitude " << d_mag - // << ", input signal power " << d_input_power - // << ", d_gnss_synchro->Acq_samplestamp_samples " << d_gnss_synchro->Acq_samplestamp_samples - // << ", d_gnss_synchro->Flag_valid_word " << d_gnss_synchro->Flag_valid_word - // << ", Flag_valid_pseudorange " << d_gnss_synchro->Flag_valid_pseudorange - // << ", d_gnss_synchro->Flag_valid_symbol_output " << d_gnss_synchro->Flag_valid_symbol_output - // << ", d_gnss_synchro->Flag_valid_acquisition " << d_gnss_synchro->Flag_valid_acquisition - // << std::endl; - //the channel FSM is set, so, notify it directly the positive acquisition to minimize delays d_channel_fsm.lock()->Event_valid_acquisition(); } @@ -271,7 +257,7 @@ void pcps_acquisition_fpga::set_active(bool active) acquisition_fpga->write_local_code(); acquisition_fpga->set_block_exp(d_total_block_exp); - acquisition_core(d_num_doppler_bins, d_doppler_step, -d_doppler_max); + acquisition_core(d_num_doppler_bins, d_doppler_step, -d_doppler_max + d_doppler_center); if (!d_make_2_steps) { acquisition_fpga->close_device(); @@ -298,7 +284,7 @@ void pcps_acquisition_fpga::set_active(bool active) while (num_second_acq < d_max_num_acqs) { - acquisition_core(d_num_doppler_bins_step2, d_doppler_step2, d_doppler_center_step_two - static_cast(floor(d_num_doppler_bins_step2 / 2.0)) * d_doppler_step2); + acquisition_core(d_num_doppler_bins_step2, d_doppler_step2, d_doppler_center_step_two - static_cast(floor(d_num_doppler_bins_step2 / 2.0)) * d_doppler_step2 + d_doppler_center); if (d_test_statistics > d_threshold) { d_active = false; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h index d1e617025..baac86cf9 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.h @@ -44,10 +44,10 @@ #include "channel_fsm.h" #include "fpga_acquisition.h" #include -#include // for lv_16sc_t #include // for uint32_t #include // for shared_ptr #include // for string +#include class Gnss_Synchro; @@ -183,6 +183,19 @@ public: acquisition_fpga->set_doppler_step(doppler_step); } + /*! + * \brief Set Doppler center frequency for the grid search. It will refresh the Doppler grid. + * \param doppler_center - Frequency center of the search grid [Hz]. + */ + inline void set_doppler_center(int32_t doppler_center) + { + if (doppler_center != d_doppler_center) + { + DLOG(INFO) << " Doppler assistance for Channel: " << d_channel << " => Doppler: " << doppler_center << "[Hz]"; + d_doppler_center = doppler_center; + } + } + /*! * \brief This function triggers a HW reset of the FPGA PL. */ @@ -196,6 +209,7 @@ private: uint32_t d_doppler_index; uint32_t d_channel; uint32_t d_doppler_step; + int32_t d_doppler_center; uint32_t d_doppler_max; uint32_t d_fft_size; uint32_t d_num_doppler_bins; From 6f656bc7447c5076e7f7b0e7a57ca58fc44551ef Mon Sep 17 00:00:00 2001 From: Marc Majoral Date: Fri, 26 Jul 2019 10:10:12 +0200 Subject: [PATCH 22/22] solved minor bug in cn0_and_tracking_lock_status --- .../gnuradio_blocks/dll_pll_veml_tracking_fpga.cc | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) 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 150ab3f27..4acb3cab4 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 @@ -597,14 +597,11 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra if (d_cn0_estimation_counter < trk_parameters.cn0_samples) { // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = d_P_accu; - d_cn0_estimation_counter++; + d_Prompt_buffer[d_cn0_estimation_counter++] = d_P_accu; return true; } - - d_cn0_estimation_counter = 0; - d_Prompt_buffer[d_cn0_estimation_counter++] = d_P_accu; - d_cn0_estimation_counter++; + d_cn0_estimation_counter = 0; + d_Prompt_buffer[d_cn0_estimation_counter++] = d_P_accu; // Code lock indicator float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer.data(), trk_parameters.cn0_samples, static_cast(coh_integration_time_s)); d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw);