From b66540b776bf0effa059af6d545f4e83e3835777 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 7 Sep 2018 00:06:06 +0200 Subject: [PATCH] Add moving average smoother to carrier phase rate --- ...2f_high_dynamic_rotator_dot_prod_32fc_xn.h | 20 +++++---------- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 25 ++++++++++++++++++- .../gnuradio_blocks/dll_pll_veml_tracking.h | 1 + src/algorithms/tracking/libs/dll_pll_conf.cc | 1 + src/algorithms/tracking/libs/dll_pll_conf.h | 1 + 5 files changed, 33 insertions(+), 15 deletions(-) diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn.h index 2599e4977..223c6acb9 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn.h @@ -74,23 +74,18 @@ #include #include #include -//#include #ifdef LV_HAVE_GENERIC static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_generic(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, const lv_32fc_t phase_inc_rate, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points) { - lv_32fc_t tmp32_1, tmp32_2; + lv_32fc_t tmp32_1; #ifdef __cplusplus - float d_arg = std::arg(phase_inc_rate) / 2.0; - // float d_arg = 0.5; //BORRAR - lv_32fc_t half_phase_inc_rate = std::exp(0.0, d_arg); + lv_32fc_t half_phase_inc_rate = std::sqrt(phase_inc_rate); #else - float d_arg = carg(phase_inc_rate) / 2.0; - // float d_arg = 0.5; //BORRAR - lv_32fc_t half_phase_inc_rate = cexp(lv_cmake(0.0, d_arg)); + lv_32fc_t half_phase_inc_rate = csqrtf(phase_inc_rate); #endif - // lv_32fc_t half_phase_inc_rate = lv_cmake(0.0, 1.0); //BORRAR + lv_32fc_t constant_rotation = phase_inc * half_phase_inc_rate; lv_32fc_t delta_phase_rate = lv_cmake(1.0, 0.0); int n_vec; unsigned int n; @@ -104,7 +99,6 @@ static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_g // Regenerate phase if (n % 256 == 0) { - //printf("Phase before regeneration %i: %f,%f Modulus: %f\n", n,lv_creal(*phase),lv_cimag(*phase), cabsf(*phase)); #ifdef __cplusplus (*phase) /= std::abs((*phase)); delta_phase_rate /= std::abs(delta_phase_rate); @@ -112,14 +106,12 @@ static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_g (*phase) /= hypotf(lv_creal(*phase), lv_cimag(*phase)); delta_phase_rate /= hypotf(lv_creal(delta_phase_rate), lv_cimag(delta_phase_rate)); #endif - //printf("Phase after regeneration %i: %f,%f Modulus: %f\n", n,lv_creal(*phase),lv_cimag(*phase), cabsf(*phase)); } - (*phase) *= (phase_inc * half_phase_inc_rate * delta_phase_rate); + (*phase) *= (constant_rotation * delta_phase_rate); delta_phase_rate *= phase_inc_rate; for (n_vec = 0; n_vec < num_a_vectors; n_vec++) { - tmp32_2 = tmp32_1 * in_a[n_vec][n]; - result[n_vec] += tmp32_2; + result[n_vec] += (tmp32_1 * in_a[n_vec][n]); } } } 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 2a4dc6aa2..594625235 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -58,6 +58,7 @@ #include #include #include +#include using google::LogMessage; @@ -405,6 +406,14 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_last_prompt = gr_complex(0.0, 0.0); d_state = 0; // initial state: standby clear_tracking_vars(); + if (trk_parameters.smoother_length > 0) + { + d_cp_history.resize(trk_parameters.smoother_length); + } + else + { + d_cp_history.resize(1); + } } @@ -450,7 +459,7 @@ void dll_pll_veml_tracking::start_tracking() d_carrier_doppler_hz = d_acq_carrier_doppler_hz; d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in; d_carrier_phase_rate_step_rad = 0.0; - + d_cp_history.clear(); // DLL/PLL filter initialization d_carrier_loop_filter.initialize(); // initialize the carrier filter d_code_loop_filter.initialize(); // initialize the code filter @@ -799,7 +808,21 @@ void dll_pll_veml_tracking::update_tracking_vars() //################### PLL COMMANDS ################################################# // carrier phase step (NCO phase increment per sample) [rads/sample] + // carrier phase difference = carrier_phase(t2) - carrier_phase(t1) + double cp_diff = -d_carrier_phase_step_rad; // The previous cp value is stored in the variable d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in; + if (trk_parameters.use_high_dynamics_resampler) + { + cp_diff += d_carrier_phase_step_rad; // The new cp value is added to the previous in order to obtain the difference + // carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2] + cp_diff /= static_cast(d_current_prn_length_samples); + d_cp_history.push_back(cp_diff); + if (d_cp_history.full()) + { + d_carrier_phase_rate_step_rad = std::accumulate(d_cp_history.begin(), d_cp_history.end(), 0.0) / static_cast(d_cp_history.size()); + } + } + // remnant carrier phase to prevent overflow in the code NCO d_rem_carr_phase_rad += d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); 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 4686fa4a8..0f5e14068 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -148,6 +148,7 @@ private: double d_code_phase_rate_step_chips; double d_carrier_phase_step_rad; double d_carrier_phase_rate_step_rad; + boost::circular_buffer d_cp_history; // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; double d_rem_carr_phase_rad; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index 525adcceb..d0da99379 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -37,6 +37,7 @@ Dll_Pll_Conf::Dll_Pll_Conf() { /* DLL/PLL tracking configuration */ use_high_dynamics_resampler = true; + smoother_length = 10; fs_in = 0.0; vector_length = 0U; dump = false; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.h b/src/algorithms/tracking/libs/dll_pll_conf.h index d557b89c0..1cf3a46eb 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.h +++ b/src/algorithms/tracking/libs/dll_pll_conf.h @@ -61,6 +61,7 @@ public: int32_t carrier_lock_det_mav_samples; int32_t cn0_min; int32_t max_lock_fail; + uint32_t smoother_length; double carrier_lock_th; bool track_pilot; char system;