From 4792bc32c6cdcee4d306386806e06a35d5bc3311 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 28 Aug 2018 22:26:26 +0200 Subject: [PATCH 01/15] Add some high dynamics variables --- .../tracking/gnuradio_blocks/dll_pll_veml_tracking.cc | 4 ++++ .../tracking/gnuradio_blocks/dll_pll_veml_tracking.h | 1 + 2 files changed, 5 insertions(+) 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 374cb910f..592bd5cde 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -355,6 +355,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl { // Extra correlator for the data component correlator_data_cpu.init(2 * trk_parameters.vector_length, 1); + correlator_data_cpu.set_high_dynamics_resampler(trk_parameters.use_high_dynamics_resampler); d_data_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); } else @@ -397,6 +398,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_phase_step_chips = 0.0; d_code_phase_rate_step_chips = 0.0; d_carrier_phase_step_rad = 0.0; + d_carrier_phase_rate_step_rad = 0.0; d_rem_code_phase_chips = 0.0; d_K_blk_samples = 0.0; d_code_phase_samples = 0.0; @@ -424,6 +426,7 @@ void dll_pll_veml_tracking::start_tracking() // 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; 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; @@ -446,6 +449,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; // DLL/PLL filter initialization d_carrier_loop_filter.initialize(); // initialize the carrier filter 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 97f0c54cc..4686fa4a8 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -147,6 +147,7 @@ private: double d_code_phase_step_chips; double d_code_phase_rate_step_chips; double d_carrier_phase_step_rad; + double d_carrier_phase_rate_step_rad; // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; double d_rem_carr_phase_rad; From 47e5ef7f3927170ba82863725e8419dd88013040 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 6 Sep 2018 17:46:55 +0200 Subject: [PATCH 02/15] Improve generic kernel --- ...2f_high_dynamic_rotator_dot_prod_32fc_xn.h | 512 ++++++++++++++++++ ...igh_dynamic_rotator_dotprodxnpuppet_32fc.h | 163 ++++++ .../volk_gnsssdr/lib/kernel_tests.h | 1 + .../gnuradio_blocks/dll_pll_veml_tracking.cc | 4 +- .../libs/cpu_multicorrelator_real_codes.cc | 27 +- .../libs/cpu_multicorrelator_real_codes.h | 2 + 6 files changed, 706 insertions(+), 3 deletions(-) create mode 100644 src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn.h create mode 100644 src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc.h 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 new file mode 100644 index 000000000..2599e4977 --- /dev/null +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn.h @@ -0,0 +1,512 @@ +/*! + * \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. + * \authors
    + *
  • Cillian O'Driscoll 2016. cillian.odriscoll(at)gmail.com + *
+ * + * VOLK_GNSSSDR kernel that multiplies N 32 bits complex vectors by a common vector, which is + * phase-rotated by phase offset and phase increment, and accumulates the results + * in N 32 bits float complex outputs. + * It is optimized to perform the N tap correlation process in GNSS receivers. + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * 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 . + * + * ------------------------------------------------------------------------- + */ + +/*! + * \page volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn + * + * \b Overview + * + * 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. + * + * Dispatcher Prototype + * \code + * void volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points); + * \endcode + * + * \b Inputs + * \li in_common: Pointer to one of the vectors to be rotated, multiplied and accumulated (reference vector). + * \li phase_inc: Phase increment = lv_cmake(cos(phase_step_rad), sin(phase_step_rad)) + * \li phase: Initial phase = lv_cmake(cos(initial_phase_rad), sin(initial_phase_rad)) + * \li in_a: Pointer to an array of pointers to multiple vectors to be multiplied and accumulated. + * \li num_a_vectors: Number of vectors to be multiplied by the reference vector and accumulated. + * \li num_points: Number of complex values to be multiplied together, accumulated and stored into \p result. + * + * \b Outputs + * \li phase: Final phase. + * \li result: Vector of \p num_a_vectors components with the multiple vectors of \p in_a rotated, multiplied by \p in_common and accumulated. + * + */ + +#ifndef INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_H +#define INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_H + + +#include +#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, tmp32_2; +#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); +#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)); +#endif + // lv_32fc_t half_phase_inc_rate = lv_cmake(0.0, 1.0); //BORRAR + lv_32fc_t delta_phase_rate = lv_cmake(1.0, 0.0); + int n_vec; + unsigned int n; + for (n_vec = 0; n_vec < num_a_vectors; n_vec++) + { + result[n_vec] = lv_cmake(0, 0); + } + for (n = 0; n < num_points; n++) + { + tmp32_1 = *in_common++ * (*phase); //if(n<10 || n >= 8108) printf("generic phase %i: %f,%f\n", n,lv_creal(*phase),lv_cimag(*phase)); + // 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); +#else + (*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); + 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; + } + } +} + +#endif /*LV_HAVE_GENERIC*/ + + +//#ifdef LV_HAVE_GENERIC +// +//static inline void volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_generic_reload(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points) +//{ +// lv_32fc_t tmp32_1, tmp32_2; +// const unsigned int ROTATOR_RELOAD = 256; +// int n_vec; +// unsigned int n; +// unsigned int j; +// for (n_vec = 0; n_vec < num_a_vectors; n_vec++) +// { +// result[n_vec] = lv_cmake(0, 0); +// } +// +// for (n = 0; n < num_points / ROTATOR_RELOAD; n++) +// { +// for (j = 0; j < ROTATOR_RELOAD; j++) +// { +// tmp32_1 = *in_common++ * (*phase); +// (*phase) *= phase_inc; +// for (n_vec = 0; n_vec < num_a_vectors; n_vec++) +// { +// tmp32_2 = tmp32_1 * in_a[n_vec][n * ROTATOR_RELOAD + j]; +// result[n_vec] += tmp32_2; +// } +// } +// /* Regenerate phase */ +//#ifdef __cplusplus +// (*phase) /= std::abs((*phase)); +//#else +// //(*phase) /= cabsf((*phase)); +// (*phase) /= hypotf(lv_creal(*phase), lv_cimag(*phase)); +//#endif +// } +// +// for (j = 0; j < num_points % ROTATOR_RELOAD; j++) +// { +// tmp32_1 = *in_common++ * (*phase); +// (*phase) *= phase_inc; +// for (n_vec = 0; n_vec < num_a_vectors; n_vec++) +// { +// tmp32_2 = tmp32_1 * in_a[n_vec][(num_points / ROTATOR_RELOAD) * ROTATOR_RELOAD + j]; +// result[n_vec] += tmp32_2; +// } +// } +//} +// +//#endif /*LV_HAVE_GENERIC*/ + +//////////////////////////// +/* + + +#ifdef LV_HAVE_AVX +#include +#include +static inline void volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_u_avx(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points) +{ + unsigned int number = 0; + int vec_ind = 0; + unsigned int i = 0; + const unsigned int sixteenthPoints = num_points / 16; + + const float* aPtr = (float*)in_common; + const float* bPtr[num_a_vectors]; + for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) + { + bPtr[vec_ind] = in_a[vec_ind]; + } + + lv_32fc_t _phase = (*phase); + lv_32fc_t wo; + + __m256 a0Val, a1Val, a2Val, a3Val; + __m256 b0Val[num_a_vectors], b1Val[num_a_vectors], b2Val[num_a_vectors], b3Val[num_a_vectors]; + __m256 x0Val[num_a_vectors], x1Val[num_a_vectors], x0loVal[num_a_vectors], x0hiVal[num_a_vectors], x1loVal[num_a_vectors], x1hiVal[num_a_vectors]; + __m256 c0Val[num_a_vectors], c1Val[num_a_vectors], c2Val[num_a_vectors], c3Val[num_a_vectors]; + + __m256 dotProdVal0[num_a_vectors]; + __m256 dotProdVal1[num_a_vectors]; + __m256 dotProdVal2[num_a_vectors]; + __m256 dotProdVal3[num_a_vectors]; + + for (vec_ind = 0; vec_ind < num_a_vectors; vec_ind++) + { + dotProdVal0[vec_ind] = _mm256_setzero_ps(); + dotProdVal1[vec_ind] = _mm256_setzero_ps(); + dotProdVal2[vec_ind] = _mm256_setzero_ps(); + dotProdVal3[vec_ind] = _mm256_setzero_ps(); + } + + // Set up the complex rotator + __m256 z0, z1, z2, z3; + __VOLK_ATTR_ALIGNED(32) + lv_32fc_t phase_vec[16]; + for (vec_ind = 0; vec_ind < 16; ++vec_ind) + { + phase_vec[vec_ind] = _phase; + _phase *= phase_inc; + } + + z0 = _mm256_load_ps((float*)phase_vec); + z1 = _mm256_load_ps((float*)(phase_vec + 4)); + z2 = _mm256_load_ps((float*)(phase_vec + 8)); + z3 = _mm256_load_ps((float*)(phase_vec + 12)); + + lv_32fc_t dz = phase_inc; + dz *= dz; + dz *= dz; + dz *= dz; + dz *= dz; // dz = phase_inc^16; + + for (vec_ind = 0; vec_ind < 4; ++vec_ind) + { + phase_vec[vec_ind] = dz; + } + + __m256 dz_reg = _mm256_load_ps((float*)phase_vec); + dz_reg = _mm256_complexnormalise_ps(dz_reg); + + for (; number < sixteenthPoints; number++) + { + a0Val = _mm256_loadu_ps(aPtr); + a1Val = _mm256_loadu_ps(aPtr + 8); + a2Val = _mm256_loadu_ps(aPtr + 16); + a3Val = _mm256_loadu_ps(aPtr + 24); + + a0Val = _mm256_complexmul_ps(a0Val, z0); + a1Val = _mm256_complexmul_ps(a1Val, z1); + a2Val = _mm256_complexmul_ps(a2Val, z2); + a3Val = _mm256_complexmul_ps(a3Val, z3); + + z0 = _mm256_complexmul_ps(z0, dz_reg); + z1 = _mm256_complexmul_ps(z1, dz_reg); + z2 = _mm256_complexmul_ps(z2, dz_reg); + z3 = _mm256_complexmul_ps(z3, dz_reg); + + for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) + { + x0Val[vec_ind] = _mm256_loadu_ps(bPtr[vec_ind]); // t0|t1|t2|t3|t4|t5|t6|t7 + x1Val[vec_ind] = _mm256_loadu_ps(bPtr[vec_ind] + 8); + x0loVal[vec_ind] = _mm256_unpacklo_ps(x0Val[vec_ind], x0Val[vec_ind]); // t0|t0|t1|t1|t4|t4|t5|t5 + x0hiVal[vec_ind] = _mm256_unpackhi_ps(x0Val[vec_ind], x0Val[vec_ind]); // t2|t2|t3|t3|t6|t6|t7|t7 + x1loVal[vec_ind] = _mm256_unpacklo_ps(x1Val[vec_ind], x1Val[vec_ind]); + x1hiVal[vec_ind] = _mm256_unpackhi_ps(x1Val[vec_ind], x1Val[vec_ind]); + + // TODO: it may be possible to rearrange swizzling to better pipeline data + b0Val[vec_ind] = _mm256_permute2f128_ps(x0loVal[vec_ind], x0hiVal[vec_ind], 0x20); // t0|t0|t1|t1|t2|t2|t3|t3 + b1Val[vec_ind] = _mm256_permute2f128_ps(x0loVal[vec_ind], x0hiVal[vec_ind], 0x31); // t4|t4|t5|t5|t6|t6|t7|t7 + b2Val[vec_ind] = _mm256_permute2f128_ps(x1loVal[vec_ind], x1hiVal[vec_ind], 0x20); + b3Val[vec_ind] = _mm256_permute2f128_ps(x1loVal[vec_ind], x1hiVal[vec_ind], 0x31); + + c0Val[vec_ind] = _mm256_mul_ps(a0Val, b0Val[vec_ind]); + c1Val[vec_ind] = _mm256_mul_ps(a1Val, b1Val[vec_ind]); + c2Val[vec_ind] = _mm256_mul_ps(a2Val, b2Val[vec_ind]); + c3Val[vec_ind] = _mm256_mul_ps(a3Val, b3Val[vec_ind]); + + dotProdVal0[vec_ind] = _mm256_add_ps(c0Val[vec_ind], dotProdVal0[vec_ind]); + dotProdVal1[vec_ind] = _mm256_add_ps(c1Val[vec_ind], dotProdVal1[vec_ind]); + dotProdVal2[vec_ind] = _mm256_add_ps(c2Val[vec_ind], dotProdVal2[vec_ind]); + dotProdVal3[vec_ind] = _mm256_add_ps(c3Val[vec_ind], dotProdVal3[vec_ind]); + + bPtr[vec_ind] += 16; + } + + // Force the rotators back onto the unit circle + if ((number % 64) == 0) + { + z0 = _mm256_complexnormalise_ps(z0); + z1 = _mm256_complexnormalise_ps(z1); + z2 = _mm256_complexnormalise_ps(z2); + z3 = _mm256_complexnormalise_ps(z3); + } + + aPtr += 32; + } + __VOLK_ATTR_ALIGNED(32) + lv_32fc_t dotProductVector[4]; + + for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) + { + dotProdVal0[vec_ind] = _mm256_add_ps(dotProdVal0[vec_ind], dotProdVal1[vec_ind]); + dotProdVal0[vec_ind] = _mm256_add_ps(dotProdVal0[vec_ind], dotProdVal2[vec_ind]); + dotProdVal0[vec_ind] = _mm256_add_ps(dotProdVal0[vec_ind], dotProdVal3[vec_ind]); + + _mm256_store_ps((float*)dotProductVector, dotProdVal0[vec_ind]); // Store the results back into the dot product vector + + result[vec_ind] = lv_cmake(0, 0); + for (i = 0; i < 4; ++i) + { + result[vec_ind] += dotProductVector[i]; + } + } + + z0 = _mm256_complexnormalise_ps(z0); + _mm256_store_ps((float*)phase_vec, z0); + _phase = phase_vec[0]; + _mm256_zeroupper(); + + number = sixteenthPoints * 16; + for (; number < num_points; number++) + { + wo = (*aPtr++) * _phase; + _phase *= phase_inc; + + for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) + { + result[vec_ind] += wo * in_a[vec_ind][number]; + } + } + + *phase = _phase; +} + +#endif LV_HAVE_AVX + + +#ifdef LV_HAVE_AVX +#include +#include +static inline void volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_a_avx(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points) +{ + unsigned int number = 0; + int vec_ind = 0; + unsigned int i = 0; + const unsigned int sixteenthPoints = num_points / 16; + + const float* aPtr = (float*)in_common; + const float* bPtr[num_a_vectors]; + for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) + { + bPtr[vec_ind] = in_a[vec_ind]; + } + + lv_32fc_t _phase = (*phase); + lv_32fc_t wo; + + __m256 a0Val, a1Val, a2Val, a3Val; + __m256 b0Val[num_a_vectors], b1Val[num_a_vectors], b2Val[num_a_vectors], b3Val[num_a_vectors]; + __m256 x0Val[num_a_vectors], x1Val[num_a_vectors], x0loVal[num_a_vectors], x0hiVal[num_a_vectors], x1loVal[num_a_vectors], x1hiVal[num_a_vectors]; + __m256 c0Val[num_a_vectors], c1Val[num_a_vectors], c2Val[num_a_vectors], c3Val[num_a_vectors]; + + __m256 dotProdVal0[num_a_vectors]; + __m256 dotProdVal1[num_a_vectors]; + __m256 dotProdVal2[num_a_vectors]; + __m256 dotProdVal3[num_a_vectors]; + + for (vec_ind = 0; vec_ind < num_a_vectors; vec_ind++) + { + dotProdVal0[vec_ind] = _mm256_setzero_ps(); + dotProdVal1[vec_ind] = _mm256_setzero_ps(); + dotProdVal2[vec_ind] = _mm256_setzero_ps(); + dotProdVal3[vec_ind] = _mm256_setzero_ps(); + } + + // Set up the complex rotator + __m256 z0, z1, z2, z3; + __VOLK_ATTR_ALIGNED(32) + lv_32fc_t phase_vec[16]; + for (vec_ind = 0; vec_ind < 16; ++vec_ind) + { + phase_vec[vec_ind] = _phase; + _phase *= phase_inc; + } + + z0 = _mm256_load_ps((float*)phase_vec); + z1 = _mm256_load_ps((float*)(phase_vec + 4)); + z2 = _mm256_load_ps((float*)(phase_vec + 8)); + z3 = _mm256_load_ps((float*)(phase_vec + 12)); + + lv_32fc_t dz = phase_inc; + dz *= dz; + dz *= dz; + dz *= dz; + dz *= dz; // dz = phase_inc^16; + + for (vec_ind = 0; vec_ind < 4; ++vec_ind) + { + phase_vec[vec_ind] = dz; + } + + __m256 dz_reg = _mm256_load_ps((float*)phase_vec); + dz_reg = _mm256_complexnormalise_ps(dz_reg); + + for (; number < sixteenthPoints; number++) + { + a0Val = _mm256_load_ps(aPtr); + a1Val = _mm256_load_ps(aPtr + 8); + a2Val = _mm256_load_ps(aPtr + 16); + a3Val = _mm256_load_ps(aPtr + 24); + + a0Val = _mm256_complexmul_ps(a0Val, z0); + a1Val = _mm256_complexmul_ps(a1Val, z1); + a2Val = _mm256_complexmul_ps(a2Val, z2); + a3Val = _mm256_complexmul_ps(a3Val, z3); + + z0 = _mm256_complexmul_ps(z0, dz_reg); + z1 = _mm256_complexmul_ps(z1, dz_reg); + z2 = _mm256_complexmul_ps(z2, dz_reg); + z3 = _mm256_complexmul_ps(z3, dz_reg); + + for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) + { + x0Val[vec_ind] = _mm256_loadu_ps(bPtr[vec_ind]); // t0|t1|t2|t3|t4|t5|t6|t7 + x1Val[vec_ind] = _mm256_loadu_ps(bPtr[vec_ind] + 8); + x0loVal[vec_ind] = _mm256_unpacklo_ps(x0Val[vec_ind], x0Val[vec_ind]); // t0|t0|t1|t1|t4|t4|t5|t5 + x0hiVal[vec_ind] = _mm256_unpackhi_ps(x0Val[vec_ind], x0Val[vec_ind]); // t2|t2|t3|t3|t6|t6|t7|t7 + x1loVal[vec_ind] = _mm256_unpacklo_ps(x1Val[vec_ind], x1Val[vec_ind]); + x1hiVal[vec_ind] = _mm256_unpackhi_ps(x1Val[vec_ind], x1Val[vec_ind]); + + // TODO: it may be possible to rearrange swizzling to better pipeline data + b0Val[vec_ind] = _mm256_permute2f128_ps(x0loVal[vec_ind], x0hiVal[vec_ind], 0x20); // t0|t0|t1|t1|t2|t2|t3|t3 + b1Val[vec_ind] = _mm256_permute2f128_ps(x0loVal[vec_ind], x0hiVal[vec_ind], 0x31); // t4|t4|t5|t5|t6|t6|t7|t7 + b2Val[vec_ind] = _mm256_permute2f128_ps(x1loVal[vec_ind], x1hiVal[vec_ind], 0x20); + b3Val[vec_ind] = _mm256_permute2f128_ps(x1loVal[vec_ind], x1hiVal[vec_ind], 0x31); + + c0Val[vec_ind] = _mm256_mul_ps(a0Val, b0Val[vec_ind]); + c1Val[vec_ind] = _mm256_mul_ps(a1Val, b1Val[vec_ind]); + c2Val[vec_ind] = _mm256_mul_ps(a2Val, b2Val[vec_ind]); + c3Val[vec_ind] = _mm256_mul_ps(a3Val, b3Val[vec_ind]); + + dotProdVal0[vec_ind] = _mm256_add_ps(c0Val[vec_ind], dotProdVal0[vec_ind]); + dotProdVal1[vec_ind] = _mm256_add_ps(c1Val[vec_ind], dotProdVal1[vec_ind]); + dotProdVal2[vec_ind] = _mm256_add_ps(c2Val[vec_ind], dotProdVal2[vec_ind]); + dotProdVal3[vec_ind] = _mm256_add_ps(c3Val[vec_ind], dotProdVal3[vec_ind]); + + bPtr[vec_ind] += 16; + } + + // Force the rotators back onto the unit circle + if ((number % 64) == 0) + { + z0 = _mm256_complexnormalise_ps(z0); + z1 = _mm256_complexnormalise_ps(z1); + z2 = _mm256_complexnormalise_ps(z2); + z3 = _mm256_complexnormalise_ps(z3); + } + + aPtr += 32; + } + __VOLK_ATTR_ALIGNED(32) + lv_32fc_t dotProductVector[4]; + + for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) + { + dotProdVal0[vec_ind] = _mm256_add_ps(dotProdVal0[vec_ind], dotProdVal1[vec_ind]); + dotProdVal0[vec_ind] = _mm256_add_ps(dotProdVal0[vec_ind], dotProdVal2[vec_ind]); + dotProdVal0[vec_ind] = _mm256_add_ps(dotProdVal0[vec_ind], dotProdVal3[vec_ind]); + + _mm256_store_ps((float*)dotProductVector, dotProdVal0[vec_ind]); // Store the results back into the dot product vector + + result[vec_ind] = lv_cmake(0, 0); + for (i = 0; i < 4; ++i) + { + result[vec_ind] += dotProductVector[i]; + } + } + + z0 = _mm256_complexnormalise_ps(z0); + _mm256_store_ps((float*)phase_vec, z0); + _phase = phase_vec[0]; + _mm256_zeroupper(); + + number = sixteenthPoints * 16; + for (; number < num_points; number++) + { + wo = (*aPtr++) * _phase; + _phase *= phase_inc; + + for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) + { + result[vec_ind] += wo * in_a[vec_ind][number]; + } + } + + *phase = _phase; +} + + +#endif LV_HAVE_AVX +*/ + +#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 new file mode 100644 index 000000000..b2bc1b740 --- /dev/null +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc.h @@ -0,0 +1,163 @@ +/*! + * \file volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc.h + * \brief Volk puppet for the multiple 16-bit complex dot product kernel. + * \authors
    + *
  • Carles Fernandez Prades 2016 cfernandez at cttc dot cat + *
+ * + * Volk puppet for integrating the resampler into volk's test system + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * 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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc_H +#define INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc_H + +#include "volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn.h" +#include +#include +#include + +#ifdef LV_HAVE_GENERIC + +static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc_generic(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(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 + +// +//#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 + +#endif // INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc_H diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/lib/kernel_tests.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/lib/kernel_tests.h index 389e03b14..51a96b171 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/lib/kernel_tests.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/lib/kernel_tests.h @@ -99,6 +99,7 @@ std::vector init_test_list(volk_gnsssdr_test_params_t QA(VOLK_INIT_PUPP(volk_gnsssdr_16ic_16i_rotator_dotprodxnpuppet_16ic, volk_gnsssdr_16ic_16i_rotator_dot_prod_16ic_xn, test_params_int16)) QA(VOLK_INIT_PUPP(volk_gnsssdr_32fc_x2_rotator_dotprodxnpuppet_32fc, volk_gnsssdr_32fc_x2_rotator_dot_prod_32fc_xn, test_params_int1)) QA(VOLK_INIT_PUPP(volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc, volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn, test_params_int1)); + QA(VOLK_INIT_PUPP(volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc, volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn, test_params_int1)); return test_cases; } 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 592bd5cde..2a4dc6aa2 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -710,7 +710,7 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, input_samples); multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( d_rem_carr_phase_rad, - d_carrier_phase_step_rad, + d_carrier_phase_step_rad, d_carrier_phase_rate_step_rad, 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), @@ -722,7 +722,7 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) correlator_data_cpu.set_input_output_vectors(d_Prompt_Data, input_samples); correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( d_rem_carr_phase_rad, - d_carrier_phase_step_rad, + d_carrier_phase_step_rad, d_carrier_phase_rate_step_rad, 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), diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc index 6222ddfb9..f23537fad 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc @@ -125,7 +125,32 @@ void cpu_multicorrelator_real_codes::update_local_code(int correlator_length_sam } } - +// Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility +bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler( + float rem_carrier_phase_in_rad, + float phase_step_rad, + float phase_rate_step_rad, + float rem_code_phase_chips, + float code_phase_step_chips, + float code_phase_rate_step_chips, + int signal_length_samples) +{ + update_local_code(signal_length_samples, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips); + // Regenerate phase at each call in order to avoid numerical issues + lv_32fc_t phase_offset_as_complex[1]; + phase_offset_as_complex[0] = lv_cmake(std::cos(rem_carrier_phase_in_rad), -std::sin(rem_carrier_phase_in_rad)); + // call VOLK_GNSSSDR kernel + if (d_use_high_dynamics_resampler) + { + volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0.0, -phase_step_rad)), std::exp(lv_32fc_t(0.0, -phase_rate_step_rad)), phase_offset_as_complex, const_cast(d_local_codes_resampled), d_n_correlators, signal_length_samples); + } + else + { + volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0.0, -phase_step_rad)), phase_offset_as_complex, const_cast(d_local_codes_resampled), d_n_correlators, signal_length_samples); + } + return true; +} +// Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler( float rem_carrier_phase_in_rad, float phase_step_rad, diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h index f7ebe7a1f..a4dfdd5f0 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h @@ -52,6 +52,8 @@ public: bool set_local_code_and_taps(int code_length_chips, const float *local_code_in, float *shifts_chips); bool set_input_output_vectors(std::complex *corr_out, const std::complex *sig_in); void update_local_code(int correlator_length_samples, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips = 0.0); + // Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility + bool Carrier_wipeoff_multicorrelator_resampler(float rem_carrier_phase_in_rad, float phase_step_rad, float phase_rate_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int signal_length_samples); bool Carrier_wipeoff_multicorrelator_resampler(float rem_carrier_phase_in_rad, float phase_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int signal_length_samples); bool free(); From b66540b776bf0effa059af6d545f4e83e3835777 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 7 Sep 2018 00:06:06 +0200 Subject: [PATCH 03/15] 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; From 2903e48e393fea51035308e00b0dd55d6abc0960 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Sat, 8 Sep 2018 23:20:29 +0200 Subject: [PATCH 04/15] Dump carrier_phase_rate and modify matlab file --- ...2f_high_dynamic_rotator_dot_prod_32fc_xn.h | 391 +----------------- .../adapters/gps_l1_ca_dll_pll_tracking.cc | 10 + .../gnuradio_blocks/dll_pll_veml_tracking.cc | 19 +- .../libs/dll_pll_veml_read_tracking_dump.m | 34 +- 4 files changed, 48 insertions(+), 406 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 223c6acb9..148fda2c3 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 @@ -86,17 +86,17 @@ static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_g 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.0, 0.0); + lv_32fc_t delta_phase_rate = 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, 0); + result[n_vec] = lv_cmake(0.0f, 0.0f); } for (n = 0; n < num_points; n++) { - tmp32_1 = *in_common++ * (*phase); //if(n<10 || n >= 8108) printf("generic phase %i: %f,%f\n", n,lv_creal(*phase),lv_cimag(*phase)); - // Regenerate phase + tmp32_1 = *in_common++ * (*phase); + // Regenerate phase if (n % 256 == 0) { #ifdef __cplusplus @@ -118,387 +118,4 @@ static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_g #endif /*LV_HAVE_GENERIC*/ - -//#ifdef LV_HAVE_GENERIC -// -//static inline void volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_generic_reload(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points) -//{ -// lv_32fc_t tmp32_1, tmp32_2; -// const unsigned int ROTATOR_RELOAD = 256; -// int n_vec; -// unsigned int n; -// unsigned int j; -// for (n_vec = 0; n_vec < num_a_vectors; n_vec++) -// { -// result[n_vec] = lv_cmake(0, 0); -// } -// -// for (n = 0; n < num_points / ROTATOR_RELOAD; n++) -// { -// for (j = 0; j < ROTATOR_RELOAD; j++) -// { -// tmp32_1 = *in_common++ * (*phase); -// (*phase) *= phase_inc; -// for (n_vec = 0; n_vec < num_a_vectors; n_vec++) -// { -// tmp32_2 = tmp32_1 * in_a[n_vec][n * ROTATOR_RELOAD + j]; -// result[n_vec] += tmp32_2; -// } -// } -// /* Regenerate phase */ -//#ifdef __cplusplus -// (*phase) /= std::abs((*phase)); -//#else -// //(*phase) /= cabsf((*phase)); -// (*phase) /= hypotf(lv_creal(*phase), lv_cimag(*phase)); -//#endif -// } -// -// for (j = 0; j < num_points % ROTATOR_RELOAD; j++) -// { -// tmp32_1 = *in_common++ * (*phase); -// (*phase) *= phase_inc; -// for (n_vec = 0; n_vec < num_a_vectors; n_vec++) -// { -// tmp32_2 = tmp32_1 * in_a[n_vec][(num_points / ROTATOR_RELOAD) * ROTATOR_RELOAD + j]; -// result[n_vec] += tmp32_2; -// } -// } -//} -// -//#endif /*LV_HAVE_GENERIC*/ - -//////////////////////////// -/* - - -#ifdef LV_HAVE_AVX -#include -#include -static inline void volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_u_avx(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points) -{ - unsigned int number = 0; - int vec_ind = 0; - unsigned int i = 0; - const unsigned int sixteenthPoints = num_points / 16; - - const float* aPtr = (float*)in_common; - const float* bPtr[num_a_vectors]; - for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) - { - bPtr[vec_ind] = in_a[vec_ind]; - } - - lv_32fc_t _phase = (*phase); - lv_32fc_t wo; - - __m256 a0Val, a1Val, a2Val, a3Val; - __m256 b0Val[num_a_vectors], b1Val[num_a_vectors], b2Val[num_a_vectors], b3Val[num_a_vectors]; - __m256 x0Val[num_a_vectors], x1Val[num_a_vectors], x0loVal[num_a_vectors], x0hiVal[num_a_vectors], x1loVal[num_a_vectors], x1hiVal[num_a_vectors]; - __m256 c0Val[num_a_vectors], c1Val[num_a_vectors], c2Val[num_a_vectors], c3Val[num_a_vectors]; - - __m256 dotProdVal0[num_a_vectors]; - __m256 dotProdVal1[num_a_vectors]; - __m256 dotProdVal2[num_a_vectors]; - __m256 dotProdVal3[num_a_vectors]; - - for (vec_ind = 0; vec_ind < num_a_vectors; vec_ind++) - { - dotProdVal0[vec_ind] = _mm256_setzero_ps(); - dotProdVal1[vec_ind] = _mm256_setzero_ps(); - dotProdVal2[vec_ind] = _mm256_setzero_ps(); - dotProdVal3[vec_ind] = _mm256_setzero_ps(); - } - - // Set up the complex rotator - __m256 z0, z1, z2, z3; - __VOLK_ATTR_ALIGNED(32) - lv_32fc_t phase_vec[16]; - for (vec_ind = 0; vec_ind < 16; ++vec_ind) - { - phase_vec[vec_ind] = _phase; - _phase *= phase_inc; - } - - z0 = _mm256_load_ps((float*)phase_vec); - z1 = _mm256_load_ps((float*)(phase_vec + 4)); - z2 = _mm256_load_ps((float*)(phase_vec + 8)); - z3 = _mm256_load_ps((float*)(phase_vec + 12)); - - lv_32fc_t dz = phase_inc; - dz *= dz; - dz *= dz; - dz *= dz; - dz *= dz; // dz = phase_inc^16; - - for (vec_ind = 0; vec_ind < 4; ++vec_ind) - { - phase_vec[vec_ind] = dz; - } - - __m256 dz_reg = _mm256_load_ps((float*)phase_vec); - dz_reg = _mm256_complexnormalise_ps(dz_reg); - - for (; number < sixteenthPoints; number++) - { - a0Val = _mm256_loadu_ps(aPtr); - a1Val = _mm256_loadu_ps(aPtr + 8); - a2Val = _mm256_loadu_ps(aPtr + 16); - a3Val = _mm256_loadu_ps(aPtr + 24); - - a0Val = _mm256_complexmul_ps(a0Val, z0); - a1Val = _mm256_complexmul_ps(a1Val, z1); - a2Val = _mm256_complexmul_ps(a2Val, z2); - a3Val = _mm256_complexmul_ps(a3Val, z3); - - z0 = _mm256_complexmul_ps(z0, dz_reg); - z1 = _mm256_complexmul_ps(z1, dz_reg); - z2 = _mm256_complexmul_ps(z2, dz_reg); - z3 = _mm256_complexmul_ps(z3, dz_reg); - - for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) - { - x0Val[vec_ind] = _mm256_loadu_ps(bPtr[vec_ind]); // t0|t1|t2|t3|t4|t5|t6|t7 - x1Val[vec_ind] = _mm256_loadu_ps(bPtr[vec_ind] + 8); - x0loVal[vec_ind] = _mm256_unpacklo_ps(x0Val[vec_ind], x0Val[vec_ind]); // t0|t0|t1|t1|t4|t4|t5|t5 - x0hiVal[vec_ind] = _mm256_unpackhi_ps(x0Val[vec_ind], x0Val[vec_ind]); // t2|t2|t3|t3|t6|t6|t7|t7 - x1loVal[vec_ind] = _mm256_unpacklo_ps(x1Val[vec_ind], x1Val[vec_ind]); - x1hiVal[vec_ind] = _mm256_unpackhi_ps(x1Val[vec_ind], x1Val[vec_ind]); - - // TODO: it may be possible to rearrange swizzling to better pipeline data - b0Val[vec_ind] = _mm256_permute2f128_ps(x0loVal[vec_ind], x0hiVal[vec_ind], 0x20); // t0|t0|t1|t1|t2|t2|t3|t3 - b1Val[vec_ind] = _mm256_permute2f128_ps(x0loVal[vec_ind], x0hiVal[vec_ind], 0x31); // t4|t4|t5|t5|t6|t6|t7|t7 - b2Val[vec_ind] = _mm256_permute2f128_ps(x1loVal[vec_ind], x1hiVal[vec_ind], 0x20); - b3Val[vec_ind] = _mm256_permute2f128_ps(x1loVal[vec_ind], x1hiVal[vec_ind], 0x31); - - c0Val[vec_ind] = _mm256_mul_ps(a0Val, b0Val[vec_ind]); - c1Val[vec_ind] = _mm256_mul_ps(a1Val, b1Val[vec_ind]); - c2Val[vec_ind] = _mm256_mul_ps(a2Val, b2Val[vec_ind]); - c3Val[vec_ind] = _mm256_mul_ps(a3Val, b3Val[vec_ind]); - - dotProdVal0[vec_ind] = _mm256_add_ps(c0Val[vec_ind], dotProdVal0[vec_ind]); - dotProdVal1[vec_ind] = _mm256_add_ps(c1Val[vec_ind], dotProdVal1[vec_ind]); - dotProdVal2[vec_ind] = _mm256_add_ps(c2Val[vec_ind], dotProdVal2[vec_ind]); - dotProdVal3[vec_ind] = _mm256_add_ps(c3Val[vec_ind], dotProdVal3[vec_ind]); - - bPtr[vec_ind] += 16; - } - - // Force the rotators back onto the unit circle - if ((number % 64) == 0) - { - z0 = _mm256_complexnormalise_ps(z0); - z1 = _mm256_complexnormalise_ps(z1); - z2 = _mm256_complexnormalise_ps(z2); - z3 = _mm256_complexnormalise_ps(z3); - } - - aPtr += 32; - } - __VOLK_ATTR_ALIGNED(32) - lv_32fc_t dotProductVector[4]; - - for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) - { - dotProdVal0[vec_ind] = _mm256_add_ps(dotProdVal0[vec_ind], dotProdVal1[vec_ind]); - dotProdVal0[vec_ind] = _mm256_add_ps(dotProdVal0[vec_ind], dotProdVal2[vec_ind]); - dotProdVal0[vec_ind] = _mm256_add_ps(dotProdVal0[vec_ind], dotProdVal3[vec_ind]); - - _mm256_store_ps((float*)dotProductVector, dotProdVal0[vec_ind]); // Store the results back into the dot product vector - - result[vec_ind] = lv_cmake(0, 0); - for (i = 0; i < 4; ++i) - { - result[vec_ind] += dotProductVector[i]; - } - } - - z0 = _mm256_complexnormalise_ps(z0); - _mm256_store_ps((float*)phase_vec, z0); - _phase = phase_vec[0]; - _mm256_zeroupper(); - - number = sixteenthPoints * 16; - for (; number < num_points; number++) - { - wo = (*aPtr++) * _phase; - _phase *= phase_inc; - - for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) - { - result[vec_ind] += wo * in_a[vec_ind][number]; - } - } - - *phase = _phase; -} - -#endif LV_HAVE_AVX - - -#ifdef LV_HAVE_AVX -#include -#include -static inline void volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_a_avx(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points) -{ - unsigned int number = 0; - int vec_ind = 0; - unsigned int i = 0; - const unsigned int sixteenthPoints = num_points / 16; - - const float* aPtr = (float*)in_common; - const float* bPtr[num_a_vectors]; - for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) - { - bPtr[vec_ind] = in_a[vec_ind]; - } - - lv_32fc_t _phase = (*phase); - lv_32fc_t wo; - - __m256 a0Val, a1Val, a2Val, a3Val; - __m256 b0Val[num_a_vectors], b1Val[num_a_vectors], b2Val[num_a_vectors], b3Val[num_a_vectors]; - __m256 x0Val[num_a_vectors], x1Val[num_a_vectors], x0loVal[num_a_vectors], x0hiVal[num_a_vectors], x1loVal[num_a_vectors], x1hiVal[num_a_vectors]; - __m256 c0Val[num_a_vectors], c1Val[num_a_vectors], c2Val[num_a_vectors], c3Val[num_a_vectors]; - - __m256 dotProdVal0[num_a_vectors]; - __m256 dotProdVal1[num_a_vectors]; - __m256 dotProdVal2[num_a_vectors]; - __m256 dotProdVal3[num_a_vectors]; - - for (vec_ind = 0; vec_ind < num_a_vectors; vec_ind++) - { - dotProdVal0[vec_ind] = _mm256_setzero_ps(); - dotProdVal1[vec_ind] = _mm256_setzero_ps(); - dotProdVal2[vec_ind] = _mm256_setzero_ps(); - dotProdVal3[vec_ind] = _mm256_setzero_ps(); - } - - // Set up the complex rotator - __m256 z0, z1, z2, z3; - __VOLK_ATTR_ALIGNED(32) - lv_32fc_t phase_vec[16]; - for (vec_ind = 0; vec_ind < 16; ++vec_ind) - { - phase_vec[vec_ind] = _phase; - _phase *= phase_inc; - } - - z0 = _mm256_load_ps((float*)phase_vec); - z1 = _mm256_load_ps((float*)(phase_vec + 4)); - z2 = _mm256_load_ps((float*)(phase_vec + 8)); - z3 = _mm256_load_ps((float*)(phase_vec + 12)); - - lv_32fc_t dz = phase_inc; - dz *= dz; - dz *= dz; - dz *= dz; - dz *= dz; // dz = phase_inc^16; - - for (vec_ind = 0; vec_ind < 4; ++vec_ind) - { - phase_vec[vec_ind] = dz; - } - - __m256 dz_reg = _mm256_load_ps((float*)phase_vec); - dz_reg = _mm256_complexnormalise_ps(dz_reg); - - for (; number < sixteenthPoints; number++) - { - a0Val = _mm256_load_ps(aPtr); - a1Val = _mm256_load_ps(aPtr + 8); - a2Val = _mm256_load_ps(aPtr + 16); - a3Val = _mm256_load_ps(aPtr + 24); - - a0Val = _mm256_complexmul_ps(a0Val, z0); - a1Val = _mm256_complexmul_ps(a1Val, z1); - a2Val = _mm256_complexmul_ps(a2Val, z2); - a3Val = _mm256_complexmul_ps(a3Val, z3); - - z0 = _mm256_complexmul_ps(z0, dz_reg); - z1 = _mm256_complexmul_ps(z1, dz_reg); - z2 = _mm256_complexmul_ps(z2, dz_reg); - z3 = _mm256_complexmul_ps(z3, dz_reg); - - for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) - { - x0Val[vec_ind] = _mm256_loadu_ps(bPtr[vec_ind]); // t0|t1|t2|t3|t4|t5|t6|t7 - x1Val[vec_ind] = _mm256_loadu_ps(bPtr[vec_ind] + 8); - x0loVal[vec_ind] = _mm256_unpacklo_ps(x0Val[vec_ind], x0Val[vec_ind]); // t0|t0|t1|t1|t4|t4|t5|t5 - x0hiVal[vec_ind] = _mm256_unpackhi_ps(x0Val[vec_ind], x0Val[vec_ind]); // t2|t2|t3|t3|t6|t6|t7|t7 - x1loVal[vec_ind] = _mm256_unpacklo_ps(x1Val[vec_ind], x1Val[vec_ind]); - x1hiVal[vec_ind] = _mm256_unpackhi_ps(x1Val[vec_ind], x1Val[vec_ind]); - - // TODO: it may be possible to rearrange swizzling to better pipeline data - b0Val[vec_ind] = _mm256_permute2f128_ps(x0loVal[vec_ind], x0hiVal[vec_ind], 0x20); // t0|t0|t1|t1|t2|t2|t3|t3 - b1Val[vec_ind] = _mm256_permute2f128_ps(x0loVal[vec_ind], x0hiVal[vec_ind], 0x31); // t4|t4|t5|t5|t6|t6|t7|t7 - b2Val[vec_ind] = _mm256_permute2f128_ps(x1loVal[vec_ind], x1hiVal[vec_ind], 0x20); - b3Val[vec_ind] = _mm256_permute2f128_ps(x1loVal[vec_ind], x1hiVal[vec_ind], 0x31); - - c0Val[vec_ind] = _mm256_mul_ps(a0Val, b0Val[vec_ind]); - c1Val[vec_ind] = _mm256_mul_ps(a1Val, b1Val[vec_ind]); - c2Val[vec_ind] = _mm256_mul_ps(a2Val, b2Val[vec_ind]); - c3Val[vec_ind] = _mm256_mul_ps(a3Val, b3Val[vec_ind]); - - dotProdVal0[vec_ind] = _mm256_add_ps(c0Val[vec_ind], dotProdVal0[vec_ind]); - dotProdVal1[vec_ind] = _mm256_add_ps(c1Val[vec_ind], dotProdVal1[vec_ind]); - dotProdVal2[vec_ind] = _mm256_add_ps(c2Val[vec_ind], dotProdVal2[vec_ind]); - dotProdVal3[vec_ind] = _mm256_add_ps(c3Val[vec_ind], dotProdVal3[vec_ind]); - - bPtr[vec_ind] += 16; - } - - // Force the rotators back onto the unit circle - if ((number % 64) == 0) - { - z0 = _mm256_complexnormalise_ps(z0); - z1 = _mm256_complexnormalise_ps(z1); - z2 = _mm256_complexnormalise_ps(z2); - z3 = _mm256_complexnormalise_ps(z3); - } - - aPtr += 32; - } - __VOLK_ATTR_ALIGNED(32) - lv_32fc_t dotProductVector[4]; - - for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) - { - dotProdVal0[vec_ind] = _mm256_add_ps(dotProdVal0[vec_ind], dotProdVal1[vec_ind]); - dotProdVal0[vec_ind] = _mm256_add_ps(dotProdVal0[vec_ind], dotProdVal2[vec_ind]); - dotProdVal0[vec_ind] = _mm256_add_ps(dotProdVal0[vec_ind], dotProdVal3[vec_ind]); - - _mm256_store_ps((float*)dotProductVector, dotProdVal0[vec_ind]); // Store the results back into the dot product vector - - result[vec_ind] = lv_cmake(0, 0); - for (i = 0; i < 4; ++i) - { - result[vec_ind] += dotProductVector[i]; - } - } - - z0 = _mm256_complexnormalise_ps(z0); - _mm256_store_ps((float*)phase_vec, z0); - _phase = phase_vec[0]; - _mm256_zeroupper(); - - number = sixteenthPoints * 16; - for (; number < num_points; number++) - { - wo = (*aPtr++) * _phase; - _phase *= phase_inc; - - for (vec_ind = 0; vec_ind < num_a_vectors; ++vec_ind) - { - result[vec_ind] += wo * in_a[vec_ind][number]; - } - } - - *phase = _phase; -} - - -#endif LV_HAVE_AVX -*/ - #endif /* INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_H */ 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 ffa6e1000..a9013e5ee 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 @@ -57,6 +57,16 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); trk_param.fs_in = fs_in; + trk_param.use_high_dynamics_resampler = configuration->property(role + ".high_dyn", false); + if (configuration->property(role + ".cp_smoother_length", 10) < 1) + { + trk_param.smoother_length = 1; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. cp_smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; + } + else + { + trk_param.smoother_length = configuration->property(role + ".cp_smoother_length", 10); + } bool dump = configuration->property(role + ".dump", false); trk_param.dump = dump; float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); 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 594625235..dd39480f2 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -811,11 +811,11 @@ void dll_pll_veml_tracking::update_tracking_vars() // 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; + 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); 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()) { @@ -974,6 +974,9 @@ void dll_pll_veml_tracking::log_data(bool integrating) // 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^2] + 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)); // PLL commands @@ -1013,7 +1016,7 @@ int32_t dll_pll_veml_tracking::save_matfile() // READ DUMP FILE std::ifstream::pos_type size; int32_t number_of_double_vars = 1; - int32_t number_of_float_vars = 17; + int32_t number_of_float_vars = 18; int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + sizeof(float) * number_of_float_vars + sizeof(uint32_t); std::ifstream dump_file; @@ -1049,6 +1052,7 @@ int32_t dll_pll_veml_tracking::save_matfile() uint64_t *PRN_start_sample_count = new uint64_t[num_epoch]; float *acc_carrier_phase_rad = new float[num_epoch]; float *carrier_doppler_hz = new float[num_epoch]; + float *carrier_doppler_rate_hz = new float[num_epoch]; float *code_freq_chips = new float[num_epoch]; float *carr_error_hz = new float[num_epoch]; float *carr_error_filt_hz = new float[num_epoch]; @@ -1076,6 +1080,7 @@ int32_t dll_pll_veml_tracking::save_matfile() dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(uint64_t)); dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_doppler_rate_hz[i]), sizeof(float)); dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); @@ -1103,6 +1108,7 @@ int32_t dll_pll_veml_tracking::save_matfile() delete[] PRN_start_sample_count; delete[] acc_carrier_phase_rad; delete[] carrier_doppler_hz; + delete[] carrier_doppler_rate_hz; delete[] code_freq_chips; delete[] carr_error_hz; delete[] carr_error_filt_hz; @@ -1166,6 +1172,10 @@ int32_t dll_pll_veml_tracking::save_matfile() Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -1217,6 +1227,7 @@ int32_t dll_pll_veml_tracking::save_matfile() delete[] PRN_start_sample_count; delete[] acc_carrier_phase_rad; delete[] carrier_doppler_hz; + delete[] carrier_doppler_rate_hz; delete[] code_freq_chips; delete[] carr_error_hz; delete[] carr_error_filt_hz; diff --git a/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m b/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m index a53dedbf9..2feea0c1e 100644 --- a/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m +++ b/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m @@ -33,7 +33,7 @@ function [GNSS_tracking] = dll_pll_veml_read_tracking_dump (filename, count) m = nargchk (1,2,nargin); -num_float_vars = 17; +num_float_vars = 18; num_unsigned_long_int_vars = 1; num_double_vars = 1; num_unsigned_int_vars = 1; @@ -114,17 +114,20 @@ else fseek(f,bytes_shift,'bof'); % move to next float v16 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); bytes_shift = bytes_shift + float_size_bytes; - fseek(f,bytes_shift,'bof'); % move to next interleaved float + fseek(f,bytes_shift,'bof'); % move to next float v17 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v18 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; fseek(f,bytes_shift,'bof'); % move to next float - v18 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes); + v19 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes); bytes_shift = bytes_shift + float_size_bytes; fseek(f,bytes_shift,'bof'); % move to next double - v19 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes); + v20 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes); bytes_shift = bytes_shift + double_size_bytes; fseek(f,bytes_shift,'bof'); % move to next unsigned int - v20 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes); + v21 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes); fclose (f); GNSS_tracking.VE = v1; @@ -137,15 +140,16 @@ else GNSS_tracking.PRN_start_sample = v8; GNSS_tracking.acc_carrier_phase_rad = v9; GNSS_tracking.carrier_doppler_hz = v10; - GNSS_tracking.code_freq_hz = v11; - GNSS_tracking.carr_error = v12; - GNSS_tracking.carr_nco = v13; - GNSS_tracking.code_error = v14; - GNSS_tracking.code_nco = v15; - GNSS_tracking.CN0_SNV_dB_Hz = v16; - GNSS_tracking.carrier_lock_test = v17; - GNSS_tracking.var1 = v18; - GNSS_tracking.var2 = v19; - GNSS_tracking.PRN = v20; + GNSS_tracking.carrier_doppler_rate_hz = v11; + GNSS_tracking.code_freq_hz = v12; + GNSS_tracking.carr_error = v13; + GNSS_tracking.carr_nco = v14; + GNSS_tracking.code_error = v15; + GNSS_tracking.code_nco = v16; + GNSS_tracking.CN0_SNV_dB_Hz = v17; + GNSS_tracking.carrier_lock_test = v18; + GNSS_tracking.var1 = v19; + GNSS_tracking.var2 = v20; + GNSS_tracking.PRN = v21; end From 25d04a94f2908b6884825dfa2d850df6a9955306 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Sun, 9 Sep 2018 13:40:31 +0200 Subject: [PATCH 05/15] Update rem_carrier_phase and minor changes --- .../adapters/gps_l1_ca_dll_pll_tracking.cc | 2 +- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 20 +++++++++++-------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 2 +- .../libs/cpu_multicorrelator_real_codes.cc | 3 ++- .../libs/cpu_multicorrelator_real_codes.h | 2 +- src/algorithms/tracking/libs/dll_pll_conf.cc | 2 +- src/algorithms/tracking/libs/dll_pll_conf.h | 2 +- .../libs/tracking_dump_reader.cc | 3 ++- .../libs/tracking_dump_reader.h | 1 + 9 files changed, 22 insertions(+), 15 deletions(-) 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 a9013e5ee..35ebe1aeb 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 @@ -57,7 +57,7 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); trk_param.fs_in = fs_in; - trk_param.use_high_dynamics_resampler = configuration->property(role + ".high_dyn", false); + trk_param.high_dyn = configuration->property(role + ".high_dyn", false); if (configuration->property(role + ".cp_smoother_length", 10) < 1) { trk_param.smoother_length = 1; 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 dd39480f2..e2da22e96 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -356,7 +356,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl { // Extra correlator for the data component correlator_data_cpu.init(2 * trk_parameters.vector_length, 1); - correlator_data_cpu.set_high_dynamics_resampler(trk_parameters.use_high_dynamics_resampler); + correlator_data_cpu.set_high_dynamics_resampler(trk_parameters.high_dyn); d_data_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); } else @@ -365,7 +365,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl } // --- Initializations --- - multicorrelator_cpu.set_high_dynamics_resampler(trk_parameters.use_high_dynamics_resampler); + multicorrelator_cpu.set_high_dynamics_resampler(trk_parameters.high_dyn); // Initial code frequency basis of NCO d_code_freq_chips = d_code_chip_rate; // Residual code phase (in chips) @@ -717,6 +717,7 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) // ################# CARRIER WIPEOFF AND CORRELATORS ############################## // perform carrier wipe-off and compute Early, Prompt and Late correlation multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, input_samples); + float tmp_f = d_rem_carr_phase_rad; multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( d_rem_carr_phase_rad, d_carrier_phase_step_rad, d_carrier_phase_rate_step_rad, @@ -730,7 +731,7 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) { correlator_data_cpu.set_input_output_vectors(d_Prompt_Data, input_samples); correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( - d_rem_carr_phase_rad, + tmp_f, d_carrier_phase_step_rad, d_carrier_phase_rate_step_rad, 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), @@ -814,7 +815,7 @@ void dll_pll_veml_tracking::update_tracking_vars() 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); - if (trk_parameters.use_high_dynamics_resampler) + if (trk_parameters.high_dyn) { d_cp_history.push_back(cp_diff); if (d_cp_history.full()) @@ -823,11 +824,14 @@ void dll_pll_veml_tracking::update_tracking_vars() } } + // Now the remnant carrier phase is computed in the Carrier Wipeoff function // 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); + //d_rem_carr_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_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_prn_length_samples); + 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)); //################### DLL COMMANDS ################################################# // code phase step (Code resampler phase increment per sample) [chips/sample] @@ -974,7 +978,7 @@ void dll_pll_veml_tracking::log_data(bool integrating) // 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^2] + // 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; 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 0f5e14068..83ba1a87e 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -151,7 +151,7 @@ private: 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; + float d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc index f23537fad..a0f6ff7c9 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc @@ -127,7 +127,7 @@ void cpu_multicorrelator_real_codes::update_local_code(int correlator_length_sam // Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler( - float rem_carrier_phase_in_rad, + float& rem_carrier_phase_in_rad, float phase_step_rad, float phase_rate_step_rad, float rem_code_phase_chips, @@ -148,6 +148,7 @@ bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler( { volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0.0, -phase_step_rad)), phase_offset_as_complex, const_cast(d_local_codes_resampled), d_n_correlators, signal_length_samples); } + rem_carrier_phase_in_rad = std::arg(std::conj(phase_offset_as_complex[0])); return true; } // Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h index a4dfdd5f0..a0c0b1f7a 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h @@ -53,7 +53,7 @@ public: bool set_input_output_vectors(std::complex *corr_out, const std::complex *sig_in); void update_local_code(int correlator_length_samples, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips = 0.0); // Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility - bool Carrier_wipeoff_multicorrelator_resampler(float rem_carrier_phase_in_rad, float phase_step_rad, float phase_rate_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int signal_length_samples); + bool Carrier_wipeoff_multicorrelator_resampler(float &rem_carrier_phase_in_rad, float phase_step_rad, float phase_rate_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int signal_length_samples); bool Carrier_wipeoff_multicorrelator_resampler(float rem_carrier_phase_in_rad, float phase_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int signal_length_samples); bool free(); diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index d0da99379..ffd50aea5 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -36,7 +36,7 @@ Dll_Pll_Conf::Dll_Pll_Conf() { /* DLL/PLL tracking configuration */ - use_high_dynamics_resampler = true; + high_dyn = true; smoother_length = 10; fs_in = 0.0; vector_length = 0U; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.h b/src/algorithms/tracking/libs/dll_pll_conf.h index 1cf3a46eb..042ee46b6 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.h +++ b/src/algorithms/tracking/libs/dll_pll_conf.h @@ -56,7 +56,7 @@ public: float early_late_space_narrow_chips; float very_early_late_space_narrow_chips; int32_t extend_correlation_symbols; - bool use_high_dynamics_resampler; + bool high_dyn; int32_t cn0_samples; int32_t carrier_lock_det_mav_samples; int32_t cn0_min; diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc index 0fdb3d7f9..24a5406d0 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc +++ b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc @@ -45,6 +45,7 @@ bool tracking_dump_reader::read_binary_obs() d_dump_file.read(reinterpret_cast(&PRN_start_sample_count), sizeof(uint64_t)); d_dump_file.read(reinterpret_cast(&acc_carrier_phase_rad), sizeof(float)); d_dump_file.read(reinterpret_cast(&carrier_doppler_hz), sizeof(float)); + d_dump_file.read(reinterpret_cast(&carrier_doppler_rate_hz_s), sizeof(float)); d_dump_file.read(reinterpret_cast(&code_freq_chips), sizeof(float)); d_dump_file.read(reinterpret_cast(&carr_error_hz), sizeof(float)); d_dump_file.read(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); @@ -83,7 +84,7 @@ int64_t tracking_dump_reader::num_epochs() { std::ifstream::pos_type size; int number_of_double_vars = 1; - int number_of_float_vars = 17; + int number_of_float_vars = 18; int epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + sizeof(float) * number_of_float_vars + sizeof(unsigned int); std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h index 437390c95..1f15232b3 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h +++ b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h @@ -63,6 +63,7 @@ public: // carrier and code frequency float carrier_doppler_hz; + float carrier_doppler_rate_hz_s; float code_freq_chips; // PLL commands From 0210f9a5cefc0e8a04b1f86de819d2267adf0450 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 10 Sep 2018 21:29:44 +0200 Subject: [PATCH 06/15] Estimate code rate chips freq --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 46 ++++++++++++++----- .../gnuradio_blocks/dll_pll_veml_tracking.h | 4 +- .../libs/tracking_dump_reader.cc | 3 +- .../libs/tracking_dump_reader.h | 1 + 4 files changed, 40 insertions(+), 14 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 e2da22e96..179f68f98 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -401,18 +401,19 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_carrier_phase_step_rad = 0.0; d_carrier_phase_rate_step_rad = 0.0; d_rem_code_phase_chips = 0.0; - d_K_blk_samples = 0.0; d_code_phase_samples = 0.0; 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); + d_carr_ph_history.resize(trk_parameters.smoother_length); + d_code_ph_history.resize(trk_parameters.smoother_length); } else { - d_cp_history.resize(1); + d_carr_ph_history.resize(1); + d_code_ph_history.resize(1); } } @@ -459,7 +460,8 @@ 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(); + d_carr_ph_history.clear(); + d_code_ph_history.clear(); // DLL/PLL filter initialization d_carrier_loop_filter.initialize(); // initialize the carrier filter d_code_loop_filter.initialize(); // initialize the code filter @@ -810,17 +812,17 @@ 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 + double tmp_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; - cp_diff += d_carrier_phase_step_rad; // The new cp value is added to the previous in order to obtain the difference + tmp_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); + tmp_diff /= static_cast(d_current_prn_length_samples); if (trk_parameters.high_dyn) { - d_cp_history.push_back(cp_diff); - if (d_cp_history.full()) + d_carr_ph_history.push_back(tmp_diff); + if (d_carr_ph_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()); + d_carrier_phase_rate_step_rad = std::accumulate(d_carr_ph_history.begin(), d_carr_ph_history.end(), 0.0) / static_cast(d_carr_ph_history.size()); } } @@ -835,7 +837,18 @@ void dll_pll_veml_tracking::update_tracking_vars() //################### DLL COMMANDS ################################################# // code phase step (Code resampler phase increment per sample) [chips/sample] + tmp_diff = -d_code_phase_step_chips; d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; + tmp_diff += d_code_phase_step_chips; + tmp_diff /= static_cast(d_current_prn_length_samples); + if (trk_parameters.high_dyn) + { + d_code_ph_history.push_back(tmp_diff); + if (d_code_ph_history.full()) + { + d_code_phase_rate_step_chips = std::accumulate(d_code_ph_history.begin(), d_code_ph_history.end(), 0.0) / static_cast(d_code_ph_history.size()); + } + } // 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_chips = d_code_freq_chips * d_rem_code_phase_samples / trk_parameters.fs_in; @@ -983,6 +996,9 @@ void dll_pll_veml_tracking::log_data(bool integrating) 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_error_hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); @@ -1020,7 +1036,7 @@ int32_t dll_pll_veml_tracking::save_matfile() // READ DUMP FILE std::ifstream::pos_type size; int32_t number_of_double_vars = 1; - int32_t number_of_float_vars = 18; + int32_t number_of_float_vars = 19; int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + sizeof(float) * number_of_float_vars + sizeof(uint32_t); std::ifstream dump_file; @@ -1058,6 +1074,7 @@ int32_t dll_pll_veml_tracking::save_matfile() float *carrier_doppler_hz = new float[num_epoch]; float *carrier_doppler_rate_hz = new float[num_epoch]; float *code_freq_chips = new float[num_epoch]; + float *code_freq_rate_chips = new float[num_epoch]; float *carr_error_hz = new float[num_epoch]; float *carr_error_filt_hz = new float[num_epoch]; float *code_error_chips = new float[num_epoch]; @@ -1086,6 +1103,7 @@ int32_t dll_pll_veml_tracking::save_matfile() dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carrier_doppler_rate_hz[i]), sizeof(float)); dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_freq_rate_chips[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(float)); @@ -1114,6 +1132,7 @@ int32_t dll_pll_veml_tracking::save_matfile() delete[] carrier_doppler_hz; delete[] carrier_doppler_rate_hz; delete[] code_freq_chips; + delete[] code_freq_rate_chips; delete[] carr_error_hz; delete[] carr_error_filt_hz; delete[] code_error_chips; @@ -1184,6 +1203,10 @@ int32_t dll_pll_veml_tracking::save_matfile() Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -1233,6 +1256,7 @@ int32_t dll_pll_veml_tracking::save_matfile() delete[] carrier_doppler_hz; delete[] carrier_doppler_rate_hz; delete[] code_freq_chips; + delete[] code_freq_rate_chips; delete[] carr_error_hz; delete[] carr_error_filt_hz; delete[] code_error_chips; 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 83ba1a87e..34e423d99 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -146,9 +146,10 @@ private: double d_code_phase_step_chips; double d_code_phase_rate_step_chips; + boost::circular_buffer d_code_ph_history; double d_carrier_phase_step_rad; double d_carrier_phase_rate_step_rad; - boost::circular_buffer d_cp_history; + boost::circular_buffer d_carr_ph_history; // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; float d_rem_carr_phase_rad; @@ -166,7 +167,6 @@ private: double d_carr_error_filt_hz; double d_code_error_chips; double d_code_error_filt_chips; - double d_K_blk_samples; double d_code_freq_chips; double d_carrier_doppler_hz; double d_acc_carrier_phase_rad; diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc index 24a5406d0..5ed6a0fec 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc +++ b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc @@ -47,6 +47,7 @@ bool tracking_dump_reader::read_binary_obs() d_dump_file.read(reinterpret_cast(&carrier_doppler_hz), sizeof(float)); d_dump_file.read(reinterpret_cast(&carrier_doppler_rate_hz_s), sizeof(float)); d_dump_file.read(reinterpret_cast(&code_freq_chips), sizeof(float)); + d_dump_file.read(reinterpret_cast(&code_freq_rate_chips), sizeof(float)); d_dump_file.read(reinterpret_cast(&carr_error_hz), sizeof(float)); d_dump_file.read(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); d_dump_file.read(reinterpret_cast(&code_error_chips), sizeof(float)); @@ -84,7 +85,7 @@ int64_t tracking_dump_reader::num_epochs() { std::ifstream::pos_type size; int number_of_double_vars = 1; - int number_of_float_vars = 18; + int number_of_float_vars = 19; int epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + sizeof(float) * number_of_float_vars + sizeof(unsigned int); std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h index 1f15232b3..790838a8a 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h +++ b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h @@ -65,6 +65,7 @@ public: float carrier_doppler_hz; float carrier_doppler_rate_hz_s; float code_freq_chips; + float code_freq_rate_chips; // PLL commands float carr_error_hz; From b3165593cf3864159b7b1cdc9d2e742ca89837e9 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 11 Sep 2018 18:56:12 +0200 Subject: [PATCH 07/15] Modify matlab trk dump reader --- .../libs/dll_pll_veml_read_tracking_dump.m | 34 +++++++++++-------- 1 file changed, 19 insertions(+), 15 deletions(-) diff --git a/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m b/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m index 2feea0c1e..698a1b5df 100644 --- a/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m +++ b/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m @@ -33,7 +33,7 @@ function [GNSS_tracking] = dll_pll_veml_read_tracking_dump (filename, count) m = nargchk (1,2,nargin); -num_float_vars = 18; +num_float_vars = 19; num_unsigned_long_int_vars = 1; num_double_vars = 1; num_unsigned_int_vars = 1; @@ -117,17 +117,20 @@ else fseek(f,bytes_shift,'bof'); % move to next float v17 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); bytes_shift = bytes_shift + float_size_bytes; - fseek(f,bytes_shift,'bof'); % move to next interleaved float + fseek(f,bytes_shift,'bof'); % move to next float v18 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v19 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; fseek(f,bytes_shift,'bof'); % move to next float - v19 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes); + v20 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes); bytes_shift = bytes_shift + float_size_bytes; fseek(f,bytes_shift,'bof'); % move to next double - v20 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes); + v21 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes); bytes_shift = bytes_shift + double_size_bytes; fseek(f,bytes_shift,'bof'); % move to next unsigned int - v21 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes); + v22 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes); fclose (f); GNSS_tracking.VE = v1; @@ -140,16 +143,17 @@ else GNSS_tracking.PRN_start_sample = v8; GNSS_tracking.acc_carrier_phase_rad = v9; GNSS_tracking.carrier_doppler_hz = v10; - GNSS_tracking.carrier_doppler_rate_hz = v11; + GNSS_tracking.carrier_doppler_rate_hz_s = v11; GNSS_tracking.code_freq_hz = v12; - GNSS_tracking.carr_error = v13; - GNSS_tracking.carr_nco = v14; - GNSS_tracking.code_error = v15; - GNSS_tracking.code_nco = v16; - GNSS_tracking.CN0_SNV_dB_Hz = v17; - GNSS_tracking.carrier_lock_test = v18; - GNSS_tracking.var1 = v19; - GNSS_tracking.var2 = v20; - GNSS_tracking.PRN = v21; + GNSS_tracking.code_freq_rate_hz_s = v13; + GNSS_tracking.carr_error = v14; + GNSS_tracking.carr_nco = v15; + GNSS_tracking.code_error = v16; + GNSS_tracking.code_nco = v17; + GNSS_tracking.CN0_SNV_dB_Hz = v18; + GNSS_tracking.carrier_lock_test = v19; + GNSS_tracking.var1 = v20; + GNSS_tracking.var2 = v21; + GNSS_tracking.PRN = v22; end From 350fa7da1107b56b9f5165d6ca2ff48bf6e8f91d Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 11 Sep 2018 20:47:32 +0200 Subject: [PATCH 08/15] Improve moving average smoothing filter for carrier phase and code phase --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 50 ++++++++++++++----- .../gnuradio_blocks/dll_pll_veml_tracking.h | 5 +- 2 files changed, 41 insertions(+), 14 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 179f68f98..1dbf913ba 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -407,8 +407,8 @@ 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); - d_code_ph_history.resize(trk_parameters.smoother_length); + d_carr_ph_history.resize(trk_parameters.smoother_length * 2); + d_code_ph_history.resize(trk_parameters.smoother_length * 2); } else { @@ -812,17 +812,30 @@ 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 tmp_diff = -d_carrier_phase_step_rad; // The previous cp value is stored in the variable + //double tmp_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; - tmp_diff += d_carrier_phase_step_rad; // The new cp value is added to the previous in order to obtain the difference + //tmp_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] - tmp_diff /= static_cast(d_current_prn_length_samples); + //tmp_diff /= static_cast(d_current_prn_length_samples); if (trk_parameters.high_dyn) { - d_carr_ph_history.push_back(tmp_diff); + d_carr_ph_history.push_back(std::pair(d_carrier_phase_step_rad, static_cast(d_current_prn_length_samples))); if (d_carr_ph_history.full()) { - d_carrier_phase_rate_step_rad = std::accumulate(d_carr_ph_history.begin(), d_carr_ph_history.end(), 0.0) / static_cast(d_carr_ph_history.size()); + //d_carr_ph_history.front().second = 0.0; + //d_carr_ph_history.at(trk_parameters.smoother_length).second = 0.0; + 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.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 /= 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; } } @@ -837,16 +850,29 @@ void dll_pll_veml_tracking::update_tracking_vars() //################### DLL COMMANDS ################################################# // code phase step (Code resampler phase increment per sample) [chips/sample] - tmp_diff = -d_code_phase_step_chips; + //tmp_diff = -d_code_phase_step_chips; d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; - tmp_diff += d_code_phase_step_chips; - tmp_diff /= static_cast(d_current_prn_length_samples); + //tmp_diff += d_code_phase_step_chips; + //tmp_diff /= static_cast(d_current_prn_length_samples); if (trk_parameters.high_dyn) { - d_code_ph_history.push_back(tmp_diff); + d_code_ph_history.push_back(std::pair(d_code_phase_step_chips, static_cast(d_current_prn_length_samples))); if (d_code_ph_history.full()) { - d_code_phase_rate_step_chips = std::accumulate(d_code_ph_history.begin(), d_code_ph_history.end(), 0.0) / static_cast(d_code_ph_history.size()); + //d_code_ph_history.front().second = 0.0; + //d_code_ph_history.at(trk_parameters.smoother_length).second = 0.0; + 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.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 /= 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] 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 34e423d99..226a540c8 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -41,6 +41,7 @@ #include #include #include +#include #include class dll_pll_veml_tracking; @@ -146,10 +147,10 @@ private: double d_code_phase_step_chips; double d_code_phase_rate_step_chips; - boost::circular_buffer d_code_ph_history; + boost::circular_buffer> d_code_ph_history; double d_carrier_phase_step_rad; double d_carrier_phase_rate_step_rad; - boost::circular_buffer d_carr_ph_history; + boost::circular_buffer> d_carr_ph_history; // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; float d_rem_carr_phase_rad; From 0031aaa973cf2c3975fc6c7954a921d1c244d4e9 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 11 Sep 2018 20:52:16 +0200 Subject: [PATCH 09/15] Set high_dynamics to false by default --- src/algorithms/tracking/libs/dll_pll_conf.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index ffd50aea5..c4b310c3d 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -36,7 +36,7 @@ Dll_Pll_Conf::Dll_Pll_Conf() { /* DLL/PLL tracking configuration */ - high_dyn = true; + high_dyn = false; smoother_length = 10; fs_in = 0.0; vector_length = 0U; From a465689f200fb97efd56f4ef1d6674bbcd583071 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 13 Sep 2018 19:18:19 +0200 Subject: [PATCH 10/15] Change rem_carrier_phase_rad computation --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 25 ++++++------------- .../libs/cpu_multicorrelator_real_codes.cc | 4 +-- .../libs/cpu_multicorrelator_real_codes.h | 2 +- 3 files changed, 11 insertions(+), 20 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 1dbf913ba..4bc1b065d 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -719,7 +719,7 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) // ################# CARRIER WIPEOFF AND CORRELATORS ############################## // perform carrier wipe-off and compute Early, Prompt and Late correlation multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, input_samples); - float tmp_f = d_rem_carr_phase_rad; + //float tmp_f = d_rem_carr_phase_rad; multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( d_rem_carr_phase_rad, d_carrier_phase_step_rad, d_carrier_phase_rate_step_rad, @@ -733,7 +733,7 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) { correlator_data_cpu.set_input_output_vectors(d_Prompt_Data, input_samples); correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( - tmp_f, + d_rem_carr_phase_rad, d_carrier_phase_step_rad, d_carrier_phase_rate_step_rad, 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), @@ -793,6 +793,10 @@ void dll_pll_veml_tracking::clear_tracking_vars() d_current_symbol = 0; d_Prompt_buffer_deque.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; + d_carr_ph_history.clear(); + d_code_ph_history.clear(); } @@ -811,19 +815,13 @@ 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 tmp_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; - //tmp_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] - //tmp_diff /= static_cast(d_current_prn_length_samples); 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))); if (d_carr_ph_history.full()) { - //d_carr_ph_history.front().second = 0.0; - //d_carr_ph_history.at(trk_parameters.smoother_length).second = 0.0; double tmp_cp1 = 0.0; double tmp_cp2 = 0.0; double tmp_samples = 0.0; @@ -838,11 +836,9 @@ void dll_pll_veml_tracking::update_tracking_vars() d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples; } } - - // Now the remnant carrier phase is computed in the Carrier Wipeoff function // 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) + 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); + 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 @@ -850,17 +846,12 @@ void dll_pll_veml_tracking::update_tracking_vars() //################### DLL COMMANDS ################################################# // code phase step (Code resampler phase increment per sample) [chips/sample] - //tmp_diff = -d_code_phase_step_chips; d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in; - //tmp_diff += d_code_phase_step_chips; - //tmp_diff /= static_cast(d_current_prn_length_samples); 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))); if (d_code_ph_history.full()) { - //d_code_ph_history.front().second = 0.0; - //d_code_ph_history.at(trk_parameters.smoother_length).second = 0.0; double tmp_cp1 = 0.0; double tmp_cp2 = 0.0; double tmp_samples = 0.0; diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc index a0f6ff7c9..cf5c35074 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc @@ -127,7 +127,7 @@ void cpu_multicorrelator_real_codes::update_local_code(int correlator_length_sam // Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler( - float& rem_carrier_phase_in_rad, + float rem_carrier_phase_in_rad, float phase_step_rad, float phase_rate_step_rad, float rem_code_phase_chips, @@ -148,7 +148,7 @@ bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler( { volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0.0, -phase_step_rad)), phase_offset_as_complex, const_cast(d_local_codes_resampled), d_n_correlators, signal_length_samples); } - rem_carrier_phase_in_rad = std::arg(std::conj(phase_offset_as_complex[0])); + //rem_carrier_phase_in_rad = std::arg(std::conj(phase_offset_as_complex[0])); return true; } // Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h index a0c0b1f7a..a4dfdd5f0 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h @@ -53,7 +53,7 @@ public: bool set_input_output_vectors(std::complex *corr_out, const std::complex *sig_in); void update_local_code(int correlator_length_samples, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips = 0.0); // Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility - bool Carrier_wipeoff_multicorrelator_resampler(float &rem_carrier_phase_in_rad, float phase_step_rad, float phase_rate_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int signal_length_samples); + bool Carrier_wipeoff_multicorrelator_resampler(float rem_carrier_phase_in_rad, float phase_step_rad, float phase_rate_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int signal_length_samples); bool Carrier_wipeoff_multicorrelator_resampler(float rem_carrier_phase_in_rad, float phase_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int signal_length_samples); bool free(); From 4375d0a051af65897c89716e6851fe1f6652f690 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 17 Sep 2018 19:19:19 +0200 Subject: [PATCH 11/15] Clean code --- ...2f_high_dynamic_rotator_dot_prod_32fc_xn.h | 19 ++++++++++--------- .../adapters/gps_l1_ca_dll_pll_tracking.cc | 6 +++--- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 1 - .../libs/cpu_multicorrelator_real_codes.cc | 1 - 4 files changed, 13 insertions(+), 14 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 148fda2c3..e20d229e3 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 @@ -3,7 +3,7 @@ * \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. * \authors
    - *
  • Cillian O'Driscoll 2016. cillian.odriscoll(at)gmail.com + *
  • Antonio Ramos 2018. antonio.ramosdet(at)gmail.com *
* * VOLK_GNSSSDR kernel that multiplies N 32 bits complex vectors by a common vector, which is @@ -37,7 +37,7 @@ */ /*! - * \page volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn + * \page volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn * * \b Overview * @@ -48,16 +48,17 @@ * * Dispatcher Prototype * \code - * void volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points); + * void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn(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); * \endcode * * \b Inputs - * \li in_common: Pointer to one of the vectors to be rotated, multiplied and accumulated (reference vector). - * \li phase_inc: Phase increment = lv_cmake(cos(phase_step_rad), sin(phase_step_rad)) - * \li phase: Initial phase = lv_cmake(cos(initial_phase_rad), sin(initial_phase_rad)) - * \li in_a: Pointer to an array of pointers to multiple vectors to be multiplied and accumulated. - * \li num_a_vectors: Number of vectors to be multiplied by the reference vector and accumulated. - * \li num_points: Number of complex values to be multiplied together, accumulated and stored into \p result. + * \li in_common: Pointer to one of the vectors to be rotated, multiplied and accumulated (reference vector). + * \li phase_inc: Phase increment = lv_cmake(cos(phase_step_rad), sin(phase_step_rad)) + * \li phase_inc_rate: Phase increment rate = lv_cmake(cos(phase_step_rate_rad), sin(phase_step_rate_rad)) + * \li phase: Initial phase = lv_cmake(cos(initial_phase_rad), sin(initial_phase_rad)) + * \li in_a: Pointer to an array of pointers to multiple vectors to be multiplied and accumulated. + * \li num_a_vectors: Number of vectors to be multiplied by the reference vector and accumulated. + * \li num_points: Number of complex values to be multiplied together, accumulated and stored into \p result. * * \b Outputs * \li phase: Final phase. 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 35ebe1aeb..b9a8b2c6e 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 @@ -58,14 +58,14 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); trk_param.fs_in = fs_in; trk_param.high_dyn = configuration->property(role + ".high_dyn", false); - if (configuration->property(role + ".cp_smoother_length", 10) < 1) + if (configuration->property(role + ".smoother_length", 10) < 1) { trk_param.smoother_length = 1; - std::cout << TEXT_RED << "WARNING: GPS L1 C/A. cp_smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; } else { - trk_param.smoother_length = configuration->property(role + ".cp_smoother_length", 10); + trk_param.smoother_length = configuration->property(role + ".smoother_length", 10); } bool dump = configuration->property(role + ".dump", false); trk_param.dump = dump; 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 4bc1b065d..068cb9534 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -719,7 +719,6 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) // ################# CARRIER WIPEOFF AND CORRELATORS ############################## // perform carrier wipe-off and compute Early, Prompt and Late correlation multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, input_samples); - //float tmp_f = d_rem_carr_phase_rad; multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( d_rem_carr_phase_rad, d_carrier_phase_step_rad, d_carrier_phase_rate_step_rad, diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc index cf5c35074..f23537fad 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc @@ -148,7 +148,6 @@ bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler( { volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0.0, -phase_step_rad)), phase_offset_as_complex, const_cast(d_local_codes_resampled), d_n_correlators, signal_length_samples); } - //rem_carrier_phase_in_rad = std::arg(std::conj(phase_offset_as_complex[0])); return true; } // Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility From 48b98165b40732e1195945dc6b0c36a77f692a12 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 17 Sep 2018 19:39:59 +0200 Subject: [PATCH 12/15] Add high dynamic functionality to L5, E1 and E5a --- .../adapters/galileo_e1_dll_pll_veml_tracking.cc | 10 ++++++++++ .../tracking/adapters/galileo_e5a_dll_pll_tracking.cc | 10 ++++++++++ .../tracking/adapters/gps_l5_dll_pll_tracking.cc | 10 ++++++++++ 3 files changed, 30 insertions(+) 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 48e821674..8a57fd975 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 @@ -59,6 +59,16 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( trk_param.fs_in = fs_in; bool dump = configuration->property(role + ".dump", false); trk_param.dump = dump; + trk_param.high_dyn = configuration->property(role + ".high_dyn", false); + if (configuration->property(role + ".smoother_length", 10) < 1) + { + trk_param.smoother_length = 1; + std::cout << TEXT_RED << "WARNING: Gal. E1. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; + } + else + { + trk_param.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) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); trk_param.pll_bw_hz = pll_bw_hz; 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 fe6f4a7d6..00b37d7c1 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc @@ -59,6 +59,16 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking( trk_param.fs_in = fs_in; bool dump = configuration->property(role + ".dump", false); trk_param.dump = dump; + trk_param.high_dyn = configuration->property(role + ".high_dyn", false); + if (configuration->property(role + ".smoother_length", 10) < 1) + { + trk_param.smoother_length = 1; + std::cout << TEXT_RED << "WARNING: Gal. E5a. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; + } + else + { + trk_param.smoother_length = configuration->property(role + ".smoother_length", 10); + } float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 20.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); trk_param.pll_bw_hz = pll_bw_hz; 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 11e2572c3..1775a7a89 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc @@ -59,6 +59,16 @@ GpsL5DllPllTracking::GpsL5DllPllTracking( trk_param.fs_in = fs_in; bool dump = configuration->property(role + ".dump", false); trk_param.dump = dump; + trk_param.high_dyn = configuration->property(role + ".high_dyn", false); + if (configuration->property(role + ".smoother_length", 10) < 1) + { + trk_param.smoother_length = 1; + std::cout << TEXT_RED << "WARNING: GPS L5. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; + } + else + { + trk_param.smoother_length = configuration->property(role + ".smoother_length", 10); + } float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); trk_param.pll_bw_hz = pll_bw_hz; From aff13c9a52095ddb989861b2cf1efc0384c7dcf1 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 18 Sep 2018 17:14:33 +0200 Subject: [PATCH 13/15] Add high dyn and smoother length flags for testing purposes --- src/tests/common-files/tracking_tests_flags.h | 4 +++- .../observables/hybrid_observables_test.cc | 17 ++++++++++++++--- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/src/tests/common-files/tracking_tests_flags.h b/src/tests/common-files/tracking_tests_flags.h index 9a5064050..3ae352ac9 100644 --- a/src/tests/common-files/tracking_tests_flags.h +++ b/src/tests/common-files/tracking_tests_flags.h @@ -41,7 +41,7 @@ DEFINE_bool(enable_external_signal_file, false, "Use an external signal file cap DEFINE_double(external_signal_acquisition_threshold, 2.5, "Threshold for satellite acquisition when external file is used"); DEFINE_int32(external_signal_acquisition_dwells, 5, "Maximum dwells count for satellite acquisition when external file is used"); DEFINE_double(external_signal_acquisition_doppler_max_hz, 5000.0, "Doppler max for satellite acquisition when external file is used"); -DEFINE_double(external_signal_acquisition_doppler_step_hz, 125, "Doppler step for satellite acquisition when external file is used"); +DEFINE_double(external_signal_acquisition_doppler_step_hz, 125.0, "Doppler step for satellite acquisition when external file is used"); DEFINE_string(signal_file, std::string("signal_out.bin"), "Path of the external signal capture file"); DEFINE_double(CN0_dBHz_start, std::numeric_limits::infinity(), "Enable noise generator and set the CN0 start sweep value [dB-Hz]"); @@ -77,6 +77,8 @@ DEFINE_double(skip_trk_transitory_s, 1.0, "Skip the initial tracking output sign //Tracking configuration DEFINE_int32(extend_correlation_symbols, 1, "Set the tracking coherent correlation to N symbols (up to 20 for GPS L1 C/A)"); +DEFINE_uint32(smoother_length, 10, "Set the moving average size for the carrier phase and code phase in case of high dynamics"); +DEFINE_bool(high_dyn, false, "Activates the code resampler and NCO generator for high dynamics"); //Test output configuration DEFINE_bool(plot_gps_l1_tracking_test, false, "Plots results of GpsL1CADllPllTrackingTest with gnuplot"); diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc index 81bb788ee..281b1c258 100644 --- a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc @@ -255,7 +255,9 @@ public: double DLL_wide_bw_hz, double PLL_narrow_bw_hz, double DLL_narrow_bw_hz, - int extend_correlation_symbols); + int extend_correlation_symbols, + uint32_t smoother_length, + bool high_dyn); gr::top_block_sptr top_block; std::shared_ptr factory; @@ -535,10 +537,17 @@ void HybridObservablesTest::configure_receiver( double DLL_wide_bw_hz, double PLL_narrow_bw_hz, double DLL_narrow_bw_hz, - int extend_correlation_symbols) + int extend_correlation_symbols, + uint32_t smoother_length, + bool high_dyn) { config = std::make_shared(); config->set_property("Tracking.dump", "true"); + if (high_dyn) + config->set_property("Tracking.high_dyn", "true"); + else + config->set_property("Tracking.high_dyn", "false"); + config->set_property("Tracking.smoother_length", std::to_string(smoother_length)); config->set_property("Tracking.dump_filename", "./tracking_ch_"); config->set_property("Tracking.implementation", implementation); config->set_property("Tracking.item_type", "gr_complex"); @@ -1293,7 +1302,9 @@ TEST_F(HybridObservablesTest, ValidationOfResults) FLAGS_DLL_bw_hz_start, FLAGS_PLL_narrow_bw_hz, FLAGS_DLL_narrow_bw_hz, - FLAGS_extend_correlation_symbols); + FLAGS_extend_correlation_symbols, + FLAGS_smoother_length, + FLAGS_high_dyn); for (unsigned int n = 0; n < gnss_synchro_vec.size(); n++) From 7e9b8ddd55716085f7a0fc913557253df354c96a Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 19 Sep 2018 00:26:02 +0200 Subject: [PATCH 14/15] Save carrier phase in observables unit test --- .../observables/hybrid_observables_test.cc | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc index 281b1c258..f9a875430 100644 --- a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc @@ -1634,6 +1634,18 @@ TEST_F(HybridObservablesTest, ValidationOfResults) measured_obs_vec.at(n).col(2).colptr(0) + measured_obs_vec.at(n).col(2).n_rows); save_mat_xy(tmp_vector_x4, tmp_vector_y4, std::string("measured_doppler_ch_" + std::to_string(n))); + std::vector tmp_vector_x5(true_obs_vec.at(n).col(0).colptr(0), + true_obs_vec.at(n).col(0).colptr(0) + true_obs_vec.at(n).col(0).n_rows); + std::vector tmp_vector_y5(true_obs_vec.at(n).col(3).colptr(0), + true_obs_vec.at(n).col(3).colptr(0) + true_obs_vec.at(n).col(3).n_rows); + save_mat_xy(tmp_vector_x5, tmp_vector_y5, std::string("true_cp_ch_" + std::to_string(n))); + + std::vector tmp_vector_x6(measured_obs_vec.at(n).col(0).colptr(0), + measured_obs_vec.at(n).col(0).colptr(0) + measured_obs_vec.at(n).col(0).n_rows); + std::vector tmp_vector_y6(measured_obs_vec.at(n).col(3).colptr(0), + measured_obs_vec.at(n).col(3).colptr(0) + measured_obs_vec.at(n).col(3).n_rows); + save_mat_xy(tmp_vector_x6, tmp_vector_y6, std::string("measured_cp_ch_" + std::to_string(n))); + if (epoch_counters_vec.at(n) > 10) //discard non-valid channels { From a2b668ef47fb8d7fc602f05de1a7aaa618968a2c Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 19 Sep 2018 00:59:13 +0200 Subject: [PATCH 15/15] Display more information in the terminal line --- .../tracking/gnuradio_blocks/dll_pll_veml_tracking.cc | 4 ++++ .../observables/hybrid_observables_test.cc | 2 ++ 2 files changed, 6 insertions(+) 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 068cb9534..95cf911f4 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -835,12 +835,16 @@ void dll_pll_veml_tracking::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 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); + //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)); //################### DLL COMMANDS ################################################# diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc index f9a875430..d0459907b 100644 --- a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc @@ -654,6 +654,8 @@ void HybridObservablesTest::configure_receiver( std::cout << "pll_bw_narrow_hz: " << config->property("Tracking.pll_bw_narrow_hz", 0.0) << " Hz\n"; std::cout << "dll_bw_narrow_hz: " << config->property("Tracking.dll_bw_narrow_hz", 0.0) << " Hz\n"; std::cout << "extend_correlation_symbols: " << config->property("Tracking.extend_correlation_symbols", 0) << " Symbols\n"; + std::cout << "high_dyn: " << config->property("Tracking.high_dyn", false) << "\n"; + std::cout << "smoother_length: " << config->property("Tracking.smoother_length", 0) << "\n"; std::cout << "*****************************************\n"; std::cout << "*****************************************\n"; }