mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-15 12:40:35 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next
This commit is contained in:
commit
1c78108ce2
@ -246,7 +246,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
|
||||
else if (obs->code[i] != CODE_NONE and obs->code[j] == CODE_NONE)
|
||||
{
|
||||
P1 += P1_C1; /* C1->P1 */
|
||||
PC = P1 + P1_P2;
|
||||
PC = P1 - P1_P2;
|
||||
}
|
||||
else if (obs->code[i] == CODE_NONE and obs->code[j] != CODE_NONE)
|
||||
{
|
||||
|
@ -0,0 +1,122 @@
|
||||
/*!
|
||||
* \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 <ul>
|
||||
* <li> Antonio Ramos 2018. antonio.ramosdet(at)gmail.com
|
||||
* </ul>
|
||||
*
|
||||
* 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 <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \page volk_gnsssdr_32fc_32f_high_dynamic_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.
|
||||
*
|
||||
* <b>Dispatcher Prototype</b>
|
||||
* \code
|
||||
* 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_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.
|
||||
* \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 <volk_gnsssdr/volk_gnsssdr.h>
|
||||
#include <volk_gnsssdr/volk_gnsssdr_malloc.h>
|
||||
#include <volk_gnsssdr/volk_gnsssdr_complex.h>
|
||||
#include <volk_gnsssdr/saturation_arithmetic.h>
|
||||
#include <math.h>
|
||||
|
||||
#ifdef LV_HAVE_GENERIC
|
||||
|
||||
static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_generic(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, const lv_32fc_t phase_inc_rate, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points)
|
||||
{
|
||||
lv_32fc_t tmp32_1;
|
||||
#ifdef __cplusplus
|
||||
lv_32fc_t half_phase_inc_rate = std::sqrt(phase_inc_rate);
|
||||
#else
|
||||
lv_32fc_t half_phase_inc_rate = csqrtf(phase_inc_rate);
|
||||
#endif
|
||||
lv_32fc_t constant_rotation = phase_inc * half_phase_inc_rate;
|
||||
lv_32fc_t delta_phase_rate = lv_cmake(1.0f, 0.0f);
|
||||
int n_vec;
|
||||
unsigned int n;
|
||||
for (n_vec = 0; n_vec < num_a_vectors; n_vec++)
|
||||
{
|
||||
result[n_vec] = lv_cmake(0.0f, 0.0f);
|
||||
}
|
||||
for (n = 0; n < num_points; n++)
|
||||
{
|
||||
tmp32_1 = *in_common++ * (*phase);
|
||||
// Regenerate phase
|
||||
if (n % 256 == 0)
|
||||
{
|
||||
#ifdef __cplusplus
|
||||
(*phase) /= std::abs((*phase));
|
||||
delta_phase_rate /= std::abs(delta_phase_rate);
|
||||
#else
|
||||
(*phase) /= hypotf(lv_creal(*phase), lv_cimag(*phase));
|
||||
delta_phase_rate /= hypotf(lv_creal(delta_phase_rate), lv_cimag(delta_phase_rate));
|
||||
#endif
|
||||
}
|
||||
(*phase) *= (constant_rotation * delta_phase_rate);
|
||||
delta_phase_rate *= phase_inc_rate;
|
||||
for (n_vec = 0; n_vec < num_a_vectors; n_vec++)
|
||||
{
|
||||
result[n_vec] += (tmp32_1 * in_a[n_vec][n]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif /*LV_HAVE_GENERIC*/
|
||||
|
||||
#endif /* INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_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 <ul>
|
||||
* <li> Carles Fernandez Prades 2016 cfernandez at cttc dot cat
|
||||
* </ul>
|
||||
*
|
||||
* 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 <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#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 <volk_gnsssdr/volk_gnsssdr_malloc.h>
|
||||
#include <volk_gnsssdr/volk_gnsssdr.h>
|
||||
#include <string.h>
|
||||
|
||||
#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
|
@ -99,6 +99,7 @@ std::vector<volk_gnsssdr_test_case_t> 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;
|
||||
}
|
||||
|
@ -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<float>(FLAGS_pll_bw_hz);
|
||||
trk_param.pll_bw_hz = pll_bw_hz;
|
||||
|
@ -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<float>(FLAGS_pll_bw_hz);
|
||||
trk_param.pll_bw_hz = pll_bw_hz;
|
||||
|
@ -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.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 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 + ".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);
|
||||
|
@ -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<float>(FLAGS_pll_bw_hz);
|
||||
trk_param.pll_bw_hz = pll_bw_hz;
|
||||
|
@ -58,6 +58,7 @@
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <numeric>
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
@ -355,6 +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.high_dyn);
|
||||
d_data_code = static_cast<float *>(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment()));
|
||||
}
|
||||
else
|
||||
@ -363,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)
|
||||
@ -397,12 +399,22 @@ 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;
|
||||
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_carr_ph_history.resize(trk_parameters.smoother_length * 2);
|
||||
d_code_ph_history.resize(trk_parameters.smoother_length * 2);
|
||||
}
|
||||
else
|
||||
{
|
||||
d_carr_ph_history.resize(1);
|
||||
d_code_ph_history.resize(1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -424,6 +436,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<double>(d_code_length_chips);
|
||||
double T_prn_mod_samples = T_prn_mod_seconds * trk_parameters.fs_in;
|
||||
@ -446,7 +459,9 @@ 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_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
|
||||
@ -706,7 +721,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<float>(d_rem_code_phase_chips) * static_cast<float>(d_code_samples_per_chip),
|
||||
static_cast<float>(d_code_phase_step_chips) * static_cast<float>(d_code_samples_per_chip),
|
||||
static_cast<float>(d_code_phase_rate_step_chips) * static_cast<float>(d_code_samples_per_chip),
|
||||
@ -718,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(
|
||||
d_rem_carr_phase_rad,
|
||||
d_carrier_phase_step_rad,
|
||||
d_carrier_phase_step_rad, d_carrier_phase_rate_step_rad,
|
||||
static_cast<float>(d_rem_code_phase_chips) * static_cast<float>(d_code_samples_per_chip),
|
||||
static_cast<float>(d_code_phase_step_chips) * static_cast<float>(d_code_samples_per_chip),
|
||||
static_cast<float>(d_code_phase_rate_step_chips) * static_cast<float>(d_code_samples_per_chip),
|
||||
@ -777,6 +792,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();
|
||||
}
|
||||
|
||||
|
||||
@ -796,15 +815,60 @@ void dll_pll_veml_tracking::update_tracking_vars()
|
||||
//################### PLL COMMANDS #################################################
|
||||
// carrier phase step (NCO phase increment per sample) [rads/sample]
|
||||
d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in;
|
||||
// carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2]
|
||||
if (trk_parameters.high_dyn)
|
||||
{
|
||||
d_carr_ph_history.push_back(std::pair<double, double>(d_carrier_phase_step_rad, static_cast<double>(d_current_prn_length_samples)));
|
||||
if (d_carr_ph_history.full())
|
||||
{
|
||||
double tmp_cp1 = 0.0;
|
||||
double tmp_cp2 = 0.0;
|
||||
double tmp_samples = 0.0;
|
||||
for (unsigned int k = 0; k < trk_parameters.smoother_length; k++)
|
||||
{
|
||||
tmp_cp1 += d_carr_ph_history.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<double>(trk_parameters.smoother_length);
|
||||
tmp_cp2 /= static_cast<double>(trk_parameters.smoother_length);
|
||||
d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples;
|
||||
}
|
||||
}
|
||||
//std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl;
|
||||
// remnant carrier phase to prevent overflow in the code NCO
|
||||
d_rem_carr_phase_rad += d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
|
||||
d_rem_carr_phase_rad += static_cast<float>(d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(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<double>(d_current_prn_length_samples);
|
||||
//double a = d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
|
||||
//double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(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<double>(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
|
||||
|
||||
//################### DLL COMMANDS #################################################
|
||||
// code phase step (Code resampler phase increment per sample) [chips/sample]
|
||||
d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in;
|
||||
if (trk_parameters.high_dyn)
|
||||
{
|
||||
d_code_ph_history.push_back(std::pair<double, double>(d_code_phase_step_chips, static_cast<double>(d_current_prn_length_samples)));
|
||||
if (d_code_ph_history.full())
|
||||
{
|
||||
double tmp_cp1 = 0.0;
|
||||
double tmp_cp2 = 0.0;
|
||||
double tmp_samples = 0.0;
|
||||
for (unsigned int k = 0; k < trk_parameters.smoother_length; k++)
|
||||
{
|
||||
tmp_cp1 += d_code_ph_history.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<double>(trk_parameters.smoother_length);
|
||||
tmp_cp2 /= static_cast<double>(trk_parameters.smoother_length);
|
||||
d_code_phase_rate_step_chips = (tmp_cp2 - tmp_cp1) / tmp_samples;
|
||||
}
|
||||
}
|
||||
// remnant code phase [chips]
|
||||
d_rem_code_phase_samples = K_blk_samples - static_cast<double>(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;
|
||||
@ -947,8 +1011,14 @@ 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<char *>(&tmp_float), sizeof(float));
|
||||
// carrier phase rate [Hz/s]
|
||||
tmp_float = d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2;
|
||||
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
|
||||
tmp_float = d_code_freq_chips;
|
||||
d_dump_file.write(reinterpret_cast<char *>(&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<char *>(&tmp_float), sizeof(float));
|
||||
// PLL commands
|
||||
tmp_float = d_carr_error_hz;
|
||||
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
|
||||
@ -986,7 +1056,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 = 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;
|
||||
@ -1022,7 +1092,9 @@ 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 *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];
|
||||
@ -1049,7 +1121,9 @@ int32_t dll_pll_veml_tracking::save_matfile()
|
||||
dump_file.read(reinterpret_cast<char *>(&PRN_start_sample_count[i]), sizeof(uint64_t));
|
||||
dump_file.read(reinterpret_cast<char *>(&acc_carrier_phase_rad[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&carrier_doppler_hz[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&carrier_doppler_rate_hz[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&code_freq_chips[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&code_freq_rate_chips[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&carr_error_hz[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&carr_error_filt_hz[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&code_error_chips[i]), sizeof(float));
|
||||
@ -1076,7 +1150,9 @@ 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[] code_freq_rate_chips;
|
||||
delete[] carr_error_hz;
|
||||
delete[] carr_error_filt_hz;
|
||||
delete[] code_error_chips;
|
||||
@ -1139,10 +1215,18 @@ 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);
|
||||
|
||||
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);
|
||||
@ -1190,7 +1274,9 @@ 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[] code_freq_rate_chips;
|
||||
delete[] carr_error_hz;
|
||||
delete[] carr_error_filt_hz;
|
||||
delete[] code_error_chips;
|
||||
|
@ -41,6 +41,7 @@
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <queue>
|
||||
#include <utility>
|
||||
#include <boost/circular_buffer.hpp>
|
||||
|
||||
class dll_pll_veml_tracking;
|
||||
@ -146,10 +147,13 @@ private:
|
||||
|
||||
double d_code_phase_step_chips;
|
||||
double d_code_phase_rate_step_chips;
|
||||
boost::circular_buffer<std::pair<double, double>> d_code_ph_history;
|
||||
double d_carrier_phase_step_rad;
|
||||
double d_carrier_phase_rate_step_rad;
|
||||
boost::circular_buffer<std::pair<double, double>> d_carr_ph_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;
|
||||
@ -164,7 +168,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;
|
||||
|
@ -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<const float**>(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<const float**>(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,
|
||||
|
@ -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<float> *corr_out, const std::complex<float> *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();
|
||||
|
||||
|
@ -36,7 +36,8 @@
|
||||
Dll_Pll_Conf::Dll_Pll_Conf()
|
||||
{
|
||||
/* DLL/PLL tracking configuration */
|
||||
use_high_dynamics_resampler = true;
|
||||
high_dyn = false;
|
||||
smoother_length = 10;
|
||||
fs_in = 0.0;
|
||||
vector_length = 0U;
|
||||
dump = false;
|
||||
|
@ -56,11 +56,12 @@ 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;
|
||||
int32_t max_lock_fail;
|
||||
uint32_t smoother_length;
|
||||
double carrier_lock_th;
|
||||
bool track_pilot;
|
||||
char system;
|
||||
|
@ -403,6 +403,7 @@ if(ENABLE_UNIT_TESTING)
|
||||
signal_generator_adapters
|
||||
pvt_gr_blocks
|
||||
signal_processing_testing_lib
|
||||
system_testing_lib
|
||||
${VOLK_GNSSSDR_LIBRARIES}
|
||||
${MATIO_LIBRARIES}
|
||||
${GNSS_SDR_TEST_OPTIONAL_LIBS}
|
||||
|
@ -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<double>::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");
|
||||
|
@ -458,3 +458,18 @@ void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_
|
||||
// Transform velocity using (2.73)
|
||||
v_eb_e = C_e_n.t() * v_eb_n;
|
||||
}
|
||||
|
||||
double great_circle_distance(double lat1, double lon1, double lat2, double lon2)
|
||||
{
|
||||
//The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes.
|
||||
// generally used geo measurement function
|
||||
double R = 6378.137; // Radius of earth in KM
|
||||
double dLat = lat2 * STRP_PI / 180.0 - lat1 * STRP_PI / 180.0;
|
||||
double dLon = lon2 * STRP_PI / 180.0 - lon1 * STRP_PI / 180.0;
|
||||
double a = sin(dLat / 2.0) * sin(dLat / 2.0) +
|
||||
cos(lat1 * STRP_PI / 180.0) * cos(lat2 * STRP_PI / 180.0) *
|
||||
sin(dLon / 2) * sin(dLon / 2.0);
|
||||
double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));
|
||||
double d = R * c;
|
||||
return d * 1000.0; // meters
|
||||
}
|
||||
|
@ -151,4 +151,10 @@ void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat
|
||||
*/
|
||||
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e);
|
||||
|
||||
/*!
|
||||
* \brief The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes.
|
||||
*/
|
||||
double great_circle_distance(double lat1, double lon1, double lat2, double lon2);
|
||||
|
||||
|
||||
#endif
|
||||
|
@ -45,7 +45,9 @@ bool tracking_dump_reader::read_binary_obs()
|
||||
d_dump_file.read(reinterpret_cast<char *>(&PRN_start_sample_count), sizeof(uint64_t));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&acc_carrier_phase_rad), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&carrier_doppler_hz), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&carrier_doppler_rate_hz_s), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&code_freq_chips), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&code_freq_rate_chips), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&carr_error_hz), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&carr_error_filt_hz), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&code_error_chips), sizeof(float));
|
||||
@ -83,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 = 17;
|
||||
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);
|
||||
|
@ -63,7 +63,9 @@ public:
|
||||
|
||||
// carrier and code frequency
|
||||
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;
|
||||
|
@ -260,7 +260,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<GNSSBlockFactory> factory;
|
||||
@ -540,10 +542,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<InMemoryConfiguration>();
|
||||
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");
|
||||
@ -650,6 +659,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";
|
||||
}
|
||||
@ -1499,7 +1510,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++)
|
||||
@ -1883,6 +1896,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<double> 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<double> 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<double> 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<double> 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
|
||||
{
|
||||
|
@ -38,6 +38,8 @@
|
||||
#include "rtklib_solver.h"
|
||||
#include "in_memory_configuration.h"
|
||||
#include "gnss_sdr_supl_client.h"
|
||||
#include "geofunctions.h"
|
||||
#include <armadillo>
|
||||
|
||||
|
||||
rtk_t configure_rtklib_options()
|
||||
@ -366,10 +368,6 @@ TEST(RTKLibSolverTest, test1)
|
||||
//
|
||||
// gnss_synchro_map[0] = tmp_obs;
|
||||
// gnss_synchro_map[0].PRN = 1;
|
||||
// gnss_synchro_map[0].RX_time = 518449.000000;
|
||||
// gnss_synchro_map[0].Pseudorange_m = 22816591.664859;
|
||||
// gnss_synchro_map[0].Carrier_Doppler_hz = -2579.334343;
|
||||
// gnss_synchro_map[0].Carrier_phase_rads = 794858.014183;
|
||||
|
||||
//load from xml (boost serialize)
|
||||
std::string file_name = path + "data/rtklib_test/obs_test1.xml";
|
||||
@ -417,10 +415,6 @@ TEST(RTKLibSolverTest, test1)
|
||||
// p_time += boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6));
|
||||
// std::cout << TEXT_MAGENTA << "Observable RX time (GPST) " << boost::posix_time::to_simple_string(p_time) << TEXT_RESET << std::endl;
|
||||
|
||||
std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
|
||||
<< " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude()
|
||||
<< " [deg], Height = " << d_ls_pvt->get_height() << " [m]" << std::endl;
|
||||
|
||||
std::cout << "RTKLIB Position at RX TOW = " << gnss_synchro_map.begin()->second.RX_time
|
||||
<< " in ECEF (X,Y,Z,t[meters]) = " << std::fixed << std::setprecision(16)
|
||||
<< d_ls_pvt->pvt_sol.rr[0] << ","
|
||||
@ -433,8 +427,32 @@ TEST(RTKLibSolverTest, test1)
|
||||
|
||||
//todo: check here the positioning error against the reference position generated with gnss-sim
|
||||
//reference position on in WGS84: Lat (deg), Long (deg) , H (m): 30.286502,120.032669,100
|
||||
arma::vec LLH = {30.286502, 120.032669, 100}; //ref position for this scenario
|
||||
|
||||
double error_LLH_m = great_circle_distance(LLH(0), LLH(1), d_ls_pvt->get_latitude(), d_ls_pvt->get_longitude());
|
||||
std::cout << "Lat, Long, H error: " << d_ls_pvt->get_latitude() - LLH(0)
|
||||
<< "," << d_ls_pvt->get_longitude() - LLH(1)
|
||||
<< "," << d_ls_pvt->get_height() - LLH(2) << " [deg,deg,meters]" << std::endl;
|
||||
|
||||
std::cout << "Haversine Great Circle error LLH distance: " << error_LLH_m << " [meters]" << std::endl;
|
||||
|
||||
arma::vec v_eb_n = {0.0, 0.0, 0.0};
|
||||
arma::vec true_r_eb_e;
|
||||
arma::vec true_v_eb_e;
|
||||
pv_Geo_to_ECEF(degtorad(LLH(0)), degtorad(LLH(1)), LLH(2), v_eb_n, true_r_eb_e, true_v_eb_e);
|
||||
|
||||
arma::vec measured_r_eb_e = {d_ls_pvt->pvt_sol.rr[0], d_ls_pvt->pvt_sol.rr[1], d_ls_pvt->pvt_sol.rr[2]};
|
||||
|
||||
arma::vec error_r_eb_e = measured_r_eb_e - true_r_eb_e;
|
||||
|
||||
std::cout << "ECEF position error vector: " << error_r_eb_e << " [meters]" << std::endl;
|
||||
|
||||
double error_3d_m = arma::norm(error_r_eb_e, 2);
|
||||
|
||||
std::cout << "3D positioning error: " << error_3d_m << " [meters]" << std::endl;
|
||||
|
||||
//check results against the test tolerance
|
||||
ASSERT_LT(error_3d_m, 1.0);
|
||||
pvt_valid = true;
|
||||
}
|
||||
}
|
||||
|
@ -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 = 19;
|
||||
num_unsigned_long_int_vars = 1;
|
||||
num_double_vars = 1;
|
||||
num_unsigned_int_vars = 1;
|
||||
@ -114,17 +114,23 @@ 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 float
|
||||
v18 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes);
|
||||
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
|
||||
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
|
||||
v19 = 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
|
||||
v20 = 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;
|
||||
@ -137,15 +143,17 @@ 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_s = v11;
|
||||
GNSS_tracking.code_freq_hz = v12;
|
||||
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
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user