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 822742daf..131509411 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 @@ -1,9 +1,10 @@ /*! * \file volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn.h * \brief VOLK_GNSSSDR kernel: multiplies N complex (32-bit float per component) vectors - * by a common vector, phase rotated and accumulates the results in N float complex outputs. + * by a common vector, phase rotated with Doppler rate and accumulates the results in N float complex outputs. * \authors * * VOLK_GNSSSDR kernel that multiplies N 32 bits complex vectors by a common vector, which is @@ -43,8 +44,8 @@ * * Rotates and multiplies the reference complex vector with an arbitrary number of other real vectors, * accumulates the results and stores them in the output vector. - * The rotation is done at a fixed rate per sample, from an initial \p phase offset. - * This function can be used for Doppler wipe-off and multiple correlator. + * The rotation is done at a variable rate per sample, from an initial \p phase offset. + * This function can be used for Doppler wipe-off and multiple correlator in the presence of Doppler rate. * * Dispatcher Prototype * \code @@ -70,24 +71,19 @@ #define INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_H -#include #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; -#ifdef __cplusplus - lv_32fc_t half_phase_inc_rate = std::sqrt(phase_inc_rate); -#else - lv_32fc_t half_phase_inc_rate = csqrtf(phase_inc_rate); -#endif - lv_32fc_t constant_rotation = phase_inc * half_phase_inc_rate; - lv_32fc_t delta_phase_rate = lv_cmake(1.0f, 0.0f); + lv_32fc_t phase_doppler_rate = lv_cmake(1.0f, 0.0f); + lv_32fc_t phase_doppler = (*phase); int n_vec; unsigned int n; for (n_vec = 0; n_vec < num_a_vectors; n_vec++) @@ -96,27 +92,60 @@ static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_g } for (n = 0; n < num_points; n++) { - tmp32_1 = *in_common++ * (*phase); // Regenerate phase if (n % 256 == 0) { #ifdef __cplusplus (*phase) /= std::abs((*phase)); - delta_phase_rate /= std::abs(delta_phase_rate); #else (*phase) /= hypotf(lv_creal(*phase), lv_cimag(*phase)); - delta_phase_rate /= hypotf(lv_creal(delta_phase_rate), lv_cimag(delta_phase_rate)); #endif } - (*phase) *= (constant_rotation * delta_phase_rate); - delta_phase_rate *= phase_inc_rate; + tmp32_1 = *in_common++ * (*phase); + + phase_doppler *= phase_inc; + phase_doppler_rate = cpowf(phase_inc_rate, lv_cmake(n * n, 0.0f)); + (*phase) = phase_doppler * phase_doppler_rate; + for (n_vec = 0; n_vec < num_a_vectors; n_vec++) { result[n_vec] += (tmp32_1 * in_a[n_vec][n]); } } } +#endif -#endif /*LV_HAVE_GENERIC*/ + +#ifdef LV_HAVE_GENERIC +static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_generic_acc(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 = lv_cmake(0.0f, 0.0f); + lv_32fc_t phase_rate_acc = lv_cmake(1.0f, 0.0f); + int n_vec; + unsigned int n; + for (n_vec = 0; n_vec < num_a_vectors; n_vec++) + { + result[n_vec] = lv_cmake(0.0f, 0.0f); + } + for (n = 0; n < num_points; n++) + { + // Regenerate phase + if (n % 256 == 0) + { +#ifdef __cplusplus + (*phase) /= std::abs((*phase)); +#else + (*phase) /= hypotf(lv_creal(*phase), lv_cimag(*phase)); +#endif + } + phase_rate_acc += phase_inc_rate; + (*phase) *= lv_cmake(cosf(phase_rate_acc), sinf(phase_rate_acc)); + for (n_vec = 0; n_vec < num_a_vectors; n_vec++) + { + result[n_vec] += (tmp32_1 * in_a[n_vec][n]); + } + } +} +#endif #endif /* INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_H */ diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc.h index ac71ab47a..bb3c8d9f8 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc.h @@ -1,6 +1,7 @@ /*! * \file volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc.h - * \brief Volk puppet for the multiple 16-bit complex dot product kernel. + * \brief VOLK_GNSSSDR kernel: multiplies N complex (32-bit float per component) vectors + * by a common vector, phase rotated with Doppler rate and accumulates the results in N float complex outputs. * \authors
    *
  • Carles Fernandez Prades 2016 cfernandez at cttc dot cat *
@@ -61,6 +62,7 @@ static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32 in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment()); memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points); } + volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_generic(result, local_code, phase_inc[0], phase_inc_rate[0], phase, (const float**)in_a, num_a_vectors, num_points); for (n = 0; n < num_a_vectors; n++) @@ -71,93 +73,35 @@ static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32 } #endif // Generic -// -//#ifdef LV_HAVE_GENERIC -//static inline void volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc_generic_reload(lv_32fc_t* result, const lv_32fc_t* local_code, const float* in, unsigned int num_points) -//{ -// // phases must be normalized. Phase rotator expects a complex exponential input! -// float rem_carrier_phase_in_rad = 0.25; -// float phase_step_rad = 0.1; -// lv_32fc_t phase[1]; -// phase[0] = lv_cmake(cos(rem_carrier_phase_in_rad), sin(rem_carrier_phase_in_rad)); -// lv_32fc_t phase_inc[1]; -// phase_inc[0] = lv_cmake(cos(phase_step_rad), sin(phase_step_rad)); -// int n; -// int num_a_vectors = 3; -// float** in_a = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_a_vectors, volk_gnsssdr_get_alignment()); -// for (n = 0; n < num_a_vectors; n++) -// { -// in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment()); -// memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points); -// } -// volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_generic_reload(result, local_code, phase_inc[0], phase, (const float**)in_a, num_a_vectors, num_points); -// -// for (n = 0; n < num_a_vectors; n++) -// { -// volk_gnsssdr_free(in_a[n]); -// } -// volk_gnsssdr_free(in_a); -//} -// -//#endif // Generic -// -//#ifdef LV_HAVE_AVX -//static inline void volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc_u_avx(lv_32fc_t* result, const lv_32fc_t* local_code, const float* in, unsigned int num_points) -//{ -// // phases must be normalized. Phase rotator expects a complex exponential input! -// float rem_carrier_phase_in_rad = 0.25; -// float phase_step_rad = 0.1; -// lv_32fc_t phase[1]; -// phase[0] = lv_cmake(cos(rem_carrier_phase_in_rad), sin(rem_carrier_phase_in_rad)); -// lv_32fc_t phase_inc[1]; -// phase_inc[0] = lv_cmake(cos(phase_step_rad), sin(phase_step_rad)); -// int n; -// int num_a_vectors = 3; -// float** in_a = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_a_vectors, volk_gnsssdr_get_alignment()); -// for (n = 0; n < num_a_vectors; n++) -// { -// in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment()); -// memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points); -// } -// volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_u_avx(result, local_code, phase_inc[0], phase, (const float**)in_a, num_a_vectors, num_points); -// -// for (n = 0; n < num_a_vectors; n++) -// { -// volk_gnsssdr_free(in_a[n]); -// } -// volk_gnsssdr_free(in_a); -//} -// -//#endif // AVX -// -// -//#ifdef LV_HAVE_AVX -//static inline void volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc_a_avx(lv_32fc_t* result, const lv_32fc_t* local_code, const float* in, unsigned int num_points) -//{ -// // phases must be normalized. Phase rotator expects a complex exponential input! -// float rem_carrier_phase_in_rad = 0.25; -// float phase_step_rad = 0.1; -// lv_32fc_t phase[1]; -// phase[0] = lv_cmake(cos(rem_carrier_phase_in_rad), sin(rem_carrier_phase_in_rad)); -// lv_32fc_t phase_inc[1]; -// phase_inc[0] = lv_cmake(cos(phase_step_rad), sin(phase_step_rad)); -// int n; -// int num_a_vectors = 3; -// float** in_a = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_a_vectors, volk_gnsssdr_get_alignment()); -// for (n = 0; n < num_a_vectors; n++) -// { -// in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment()); -// memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points); -// } -// volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_a_avx(result, local_code, phase_inc[0], phase, (const float**)in_a, num_a_vectors, num_points); -// -// for (n = 0; n < num_a_vectors; n++) -// { -// volk_gnsssdr_free(in_a[n]); -// } -// volk_gnsssdr_free(in_a); -//} -// -//#endif // AVX +#ifdef LV_HAVE_GENERIC +static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc_generic_acc(lv_32fc_t* result, const lv_32fc_t* local_code, const float* in, unsigned int num_points) +{ + // phases must be normalized. Phase rotator expects a complex exponential input! + float rem_carrier_phase_in_rad = 0.25; + float phase_step_rad = 0.1; + lv_32fc_t phase[1]; + phase[0] = lv_cmake(cos(rem_carrier_phase_in_rad), sin(rem_carrier_phase_in_rad)); + lv_32fc_t phase_inc[1]; + phase_inc[0] = lv_cmake(cos(phase_step_rad), sin(phase_step_rad)); + lv_32fc_t phase_inc_rate[1]; + phase_inc_rate[0] = lv_cmake(cos(phase_step_rad * 0.001), sin(phase_step_rad * 0.001)); + int n; + int num_a_vectors = 3; + float** in_a = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_a_vectors, volk_gnsssdr_get_alignment()); + for (n = 0; n < num_a_vectors; n++) + { + in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment()); + memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points); + } + + volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_generic_acc(result, local_code, phase_inc[0], phase_inc_rate[0], phase, (const float**)in_a, num_a_vectors, num_points); + + for (n = 0; n < num_a_vectors; n++) + { + volk_gnsssdr_free(in_a[n]); + } + volk_gnsssdr_free(in_a); +} +#endif // Generic #endif // INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc_H diff --git a/src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc index 6dba51ede..8bfcbe82f 100644 --- a/src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc @@ -73,6 +73,49 @@ BeidouB1iDllPllTracking::BeidouB1iDllPllTracking( dll_bw_hz = static_cast(FLAGS_dll_bw_hz); } trk_param.dll_bw_hz = dll_bw_hz; + + int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); + if (dll_filter_order < 1) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 1."; + dll_filter_order = 1; + } + if (dll_filter_order > 3) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 3."; + dll_filter_order = 3; + } + trk_param.dll_filter_order = dll_filter_order; + + int pll_filter_order = configuration->property(role + ".pll_filter_order", 3); + if (pll_filter_order < 2) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 2."; + pll_filter_order = 2; + } + if (pll_filter_order > 3) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 3."; + pll_filter_order = 3; + } + trk_param.pll_filter_order = pll_filter_order; + + if (pll_filter_order == 2) + { + trk_param.fll_filter_order = 1; + } + if (pll_filter_order == 3) + { + trk_param.fll_filter_order = 2; + } + + bool enable_fll_pull_in = configuration->property(role + ".enable_fll_pull_in", false); + trk_param.enable_fll_pull_in = enable_fll_pull_in; + float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); + trk_param.fll_bw_hz = fll_bw_hz; + float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); + trk_param.pull_in_time_s = pull_in_time_s; + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); trk_param.early_late_space_chips = early_late_space_chips; float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); diff --git a/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc index 6253788e8..49d76417c 100644 --- a/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc @@ -68,6 +68,49 @@ BeidouB3iDllPllTracking::BeidouB3iDllPllTracking( float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); trk_param.dll_bw_hz = dll_bw_hz; + + int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); + if (dll_filter_order < 1) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 1."; + dll_filter_order = 1; + } + if (dll_filter_order > 3) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 3."; + dll_filter_order = 3; + } + trk_param.dll_filter_order = dll_filter_order; + + int pll_filter_order = configuration->property(role + ".pll_filter_order", 3); + if (pll_filter_order < 2) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 2."; + pll_filter_order = 2; + } + if (pll_filter_order > 3) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 3."; + pll_filter_order = 3; + } + trk_param.pll_filter_order = pll_filter_order; + + if (pll_filter_order == 2) + { + trk_param.fll_filter_order = 1; + } + if (pll_filter_order == 3) + { + trk_param.fll_filter_order = 2; + } + + bool enable_fll_pull_in = configuration->property(role + ".enable_fll_pull_in", false); + trk_param.enable_fll_pull_in = enable_fll_pull_in; + float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); + trk_param.fll_bw_hz = fll_bw_hz; + float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); + trk_param.pull_in_time_s = pull_in_time_s; + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); trk_param.early_late_space_chips = early_late_space_chips; float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); @@ -141,8 +184,7 @@ BeidouB3iDllPllTracking::BeidouB3iDllPllTracking( } -BeidouB3iDllPllTracking::~BeidouB3iDllPllTracking() -= default; +BeidouB3iDllPllTracking::~BeidouB3iDllPllTracking() = default; void BeidouB3iDllPllTracking::start_tracking() diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index ec8e95979..1beba6c51 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -88,6 +88,49 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz; + + int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); + if (dll_filter_order < 1) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 1."; + dll_filter_order = 1; + } + if (dll_filter_order > 3) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 3."; + dll_filter_order = 3; + } + trk_param.dll_filter_order = dll_filter_order; + + int pll_filter_order = configuration->property(role + ".pll_filter_order", 3); + if (pll_filter_order < 2) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 2."; + pll_filter_order = 2; + } + if (pll_filter_order > 3) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 3."; + pll_filter_order = 3; + } + trk_param.pll_filter_order = pll_filter_order; + + if (pll_filter_order == 2) + { + trk_param.fll_filter_order = 1; + } + if (pll_filter_order == 3) + { + trk_param.fll_filter_order = 2; + } + + bool enable_fll_pull_in = configuration->property(role + ".enable_fll_pull_in", false); + trk_param.enable_fll_pull_in = enable_fll_pull_in; + float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); + trk_param.fll_bw_hz = fll_bw_hz; + float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); + trk_param.pull_in_time_s = pull_in_time_s; + int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.15); trk_param.early_late_space_chips = early_late_space_chips; diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc index aa8b0f998..cdf4d7cad 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc @@ -83,6 +83,49 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking( dll_bw_hz = static_cast(FLAGS_dll_bw_hz); } trk_param.dll_bw_hz = dll_bw_hz; + + int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); + if (dll_filter_order < 1) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 1."; + dll_filter_order = 1; + } + if (dll_filter_order > 3) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 3."; + dll_filter_order = 3; + } + trk_param.dll_filter_order = dll_filter_order; + + int pll_filter_order = configuration->property(role + ".pll_filter_order", 3); + if (pll_filter_order < 2) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 2."; + pll_filter_order = 2; + } + if (pll_filter_order > 3) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 3."; + pll_filter_order = 3; + } + trk_param.pll_filter_order = pll_filter_order; + + if (pll_filter_order == 2) + { + trk_param.fll_filter_order = 1; + } + if (pll_filter_order == 3) + { + trk_param.fll_filter_order = 2; + } + + bool enable_fll_pull_in = configuration->property(role + ".enable_fll_pull_in", false); + trk_param.enable_fll_pull_in = enable_fll_pull_in; + float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); + trk_param.fll_bw_hz = fll_bw_hz; + float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); + trk_param.pull_in_time_s = pull_in_time_s; + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 5.0); trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index b8f4815e2..a511503fb 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -89,6 +89,49 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( dll_bw_hz = static_cast(FLAGS_dll_bw_hz); } trk_param.dll_bw_hz = dll_bw_hz; + + int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); + if (dll_filter_order < 1) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 1."; + dll_filter_order = 1; + } + if (dll_filter_order > 3) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 3."; + dll_filter_order = 3; + } + trk_param.dll_filter_order = dll_filter_order; + + int pll_filter_order = configuration->property(role + ".pll_filter_order", 3); + if (pll_filter_order < 2) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 2."; + pll_filter_order = 2; + } + if (pll_filter_order > 3) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 3."; + pll_filter_order = 3; + } + trk_param.pll_filter_order = pll_filter_order; + + if (pll_filter_order == 2) + { + trk_param.fll_filter_order = 1; + } + if (pll_filter_order == 3) + { + trk_param.fll_filter_order = 2; + } + + bool enable_fll_pull_in = configuration->property(role + ".enable_fll_pull_in", false); + trk_param.enable_fll_pull_in = enable_fll_pull_in; + float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); + trk_param.fll_bw_hz = fll_bw_hz; + float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); + trk_param.pull_in_time_s = pull_in_time_s; + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); trk_param.early_late_space_chips = early_late_space_chips; float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc index a1e2c722c..65912ccbf 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc @@ -77,6 +77,49 @@ GpsL2MDllPllTracking::GpsL2MDllPllTracking( float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); trk_param.early_late_space_chips = early_late_space_chips; trk_param.early_late_space_narrow_chips = 0.0; + + int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); + if (dll_filter_order < 1) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 1."; + dll_filter_order = 1; + } + if (dll_filter_order > 3) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 3."; + dll_filter_order = 3; + } + trk_param.dll_filter_order = dll_filter_order; + + int pll_filter_order = configuration->property(role + ".pll_filter_order", 3); + if (pll_filter_order < 2) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 2."; + pll_filter_order = 2; + } + if (pll_filter_order > 3) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 3."; + pll_filter_order = 3; + } + trk_param.pll_filter_order = pll_filter_order; + + if (pll_filter_order == 2) + { + trk_param.fll_filter_order = 1; + } + if (pll_filter_order == 3) + { + trk_param.fll_filter_order = 2; + } + + bool enable_fll_pull_in = configuration->property(role + ".enable_fll_pull_in", false); + trk_param.enable_fll_pull_in = enable_fll_pull_in; + float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); + trk_param.fll_bw_hz = fll_bw_hz; + float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); + trk_param.pull_in_time_s = pull_in_time_s; + int vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L2_M_CODE_RATE_HZ) / static_cast(GPS_L2_M_CODE_LENGTH_CHIPS))); trk_param.vector_length = vector_length; int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc index 2066f99be..988f8dccb 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc @@ -90,6 +90,49 @@ GpsL5DllPllTracking::GpsL5DllPllTracking( trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz; float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); trk_param.early_late_space_chips = early_late_space_chips; + + int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); + if (dll_filter_order < 1) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 1."; + dll_filter_order = 1; + } + if (dll_filter_order > 3) + { + LOG(WARNING) << "dll_filter_order parameter must be 1, 2 or 3. Set to 3."; + dll_filter_order = 3; + } + trk_param.dll_filter_order = dll_filter_order; + + int pll_filter_order = configuration->property(role + ".pll_filter_order", 3); + if (pll_filter_order < 2) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 2."; + pll_filter_order = 2; + } + if (pll_filter_order > 3) + { + LOG(WARNING) << "pll_filter_order parameter must be 2 or 3. Set to 3."; + pll_filter_order = 3; + } + trk_param.pll_filter_order = pll_filter_order; + + if (pll_filter_order == 2) + { + trk_param.fll_filter_order = 1; + } + if (pll_filter_order == 3) + { + trk_param.fll_filter_order = 2; + } + + bool enable_fll_pull_in = configuration->property(role + ".enable_fll_pull_in", false); + trk_param.enable_fll_pull_in = enable_fll_pull_in; + float fll_bw_hz = configuration->property(role + ".fll_bw_hz", 35.0); + trk_param.fll_bw_hz = fll_bw_hz; + float pull_in_time_s = configuration->property(role + ".pull_in_time_s", 2.0); + trk_param.pull_in_time_s = pull_in_time_s; + int vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L5I_CODE_RATE_HZ) / static_cast(GPS_L5I_CODE_LENGTH_CHIPS))); trk_param.vector_length = vector_length; int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1);