From e6d2776f1b4bcce6f9f008babe802ee235822aab Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 28 Feb 2019 14:10:44 +0100 Subject: [PATCH] Replace std::deque by faster boost::circular_buffer --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 48 +++++++++---------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 10 ++-- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 033e49493..9fea05e98 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -153,8 +153,8 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl n++; } } - d_symbol_history.resize(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size - d_symbol_history.clear(); // Clear all the elements in the buffer + 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") { @@ -405,6 +405,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl } // --- Initializations --- + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); multicorrelator_cpu.set_high_dynamics_resampler(trk_parameters.high_dyn); // Initial code frequency basis of NCO d_code_freq_chips = d_code_chip_rate; @@ -446,13 +447,13 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl clear_tracking_vars(); if (trk_parameters.smoother_length > 0) { - d_carr_ph_history.resize(trk_parameters.smoother_length * 2); - d_code_ph_history.resize(trk_parameters.smoother_length * 2); + d_carr_ph_history.set_capacity(trk_parameters.smoother_length * 2); + d_code_ph_history.set_capacity(trk_parameters.smoother_length * 2); } else { - d_carr_ph_history.resize(1); - d_code_ph_history.resize(1); + d_carr_ph_history.set_capacity(1); + d_code_ph_history.set_capacity(1); } d_dump = trk_parameters.dump; @@ -607,7 +608,7 @@ void dll_pll_veml_tracking::start_tracking() n++; } } - d_symbol_history.resize(22); // Change fixed buffer size + d_symbol_history.set_capacity(22); // Change fixed buffer size d_symbol_history.clear(); } } @@ -649,7 +650,7 @@ void dll_pll_veml_tracking::start_tracking() // enable tracking pull-in d_state = 1; d_cloop = true; - d_Prompt_buffer_deque.clear(); + d_Prompt_circular_buffer.clear(); d_last_prompt = gr_complex(0.0, 0.0); } @@ -710,7 +711,7 @@ bool dll_pll_veml_tracking::acquire_secondary() int32_t corr_value = 0; for (uint32_t i = 0; i < d_secondary_code_length; i++) { - if (d_Prompt_buffer_deque.at(i).real() < 0.0) // symbols clipping + if (d_Prompt_circular_buffer[i].real() < 0.0) // symbols clipping { if (d_secondary_code_string->at(i) == '0') { @@ -866,7 +867,7 @@ void dll_pll_veml_tracking::clear_tracking_vars() d_code_error_chips = 0.0; d_code_error_filt_chips = 0.0; d_current_symbol = 0; - d_Prompt_buffer_deque.clear(); + d_Prompt_circular_buffer.clear(); d_last_prompt = gr_complex(0.0, 0.0); d_carrier_phase_rate_step_rad = 0.0; d_code_phase_rate_step_chips = 0.0; @@ -902,9 +903,9 @@ void dll_pll_veml_tracking::update_tracking_vars() double tmp_samples = 0.0; for (unsigned int k = 0; k < trk_parameters.smoother_length; k++) { - tmp_cp1 += d_carr_ph_history.at(k).first; - tmp_cp2 += d_carr_ph_history.at(trk_parameters.smoother_length * 2 - k - 1).first; - tmp_samples += d_carr_ph_history.at(trk_parameters.smoother_length * 2 - k - 1).second; + 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); @@ -916,7 +917,6 @@ void dll_pll_veml_tracking::update_tracking_vars() 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 = 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); @@ -936,9 +936,9 @@ void dll_pll_veml_tracking::update_tracking_vars() double tmp_samples = 0.0; for (unsigned int k = 0; k < trk_parameters.smoother_length; k++) { - tmp_cp1 += d_code_ph_history.at(k).first; - tmp_cp2 += d_code_ph_history.at(trk_parameters.smoother_length * 2 - k - 1).first; - tmp_samples += d_code_ph_history.at(trk_parameters.smoother_length * 2 - k - 1).second; + 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); @@ -1503,8 +1503,8 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) if (d_secondary) { // ####### SECONDARY CODE LOCK ##### - d_Prompt_buffer_deque.push_back(*d_Prompt); - if (d_Prompt_buffer_deque.size() == d_secondary_code_length) + 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) @@ -1514,8 +1514,6 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) 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; } - - d_Prompt_buffer_deque.pop_front(); } } else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change @@ -1528,9 +1526,10 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) 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)) { - for (int32_t i = 0; i < d_preamble_length_symbols; i++) + int i = 0; + for (const auto &iter : d_symbol_history) { - if (d_symbol_history.at(i) < 0) // symbols clipping + if (iter < 0.0) // symbols clipping { corr_value -= d_preambles_symbols[i]; } @@ -1538,6 +1537,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { corr_value += d_preambles_symbols[i]; } + i++; } } if (corr_value == d_preamble_length_symbols) @@ -1606,7 +1606,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_L_accu = gr_complex(0.0, 0.0); d_VL_accu = gr_complex(0.0, 0.0); d_last_prompt = gr_complex(0.0, 0.0); - d_Prompt_buffer_deque.clear(); + d_Prompt_circular_buffer.clear(); d_current_symbol = 0; if (d_enable_extended_integration) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 477880b03..bcf605eb5 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -92,7 +92,7 @@ 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; @@ -111,9 +111,10 @@ private: int32_t d_preamble_length_symbols; boost::circular_buffer d_symbol_history; - //tracking state machine + // tracking state machine int32_t d_state; - //Integration period in samples + + // Integration period in samples int32_t d_correlation_length_ms; int32_t d_n_correlator_taps; @@ -188,11 +189,10 @@ private: // 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; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; - std::deque d_Prompt_buffer_deque; + boost::circular_buffer d_Prompt_circular_buffer; gr_complex *d_Prompt_buffer; // file dump