mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-15 04:30:33 +00:00
Fix high_dynamics correlator
This commit is contained in:
parent
6a17a33d6f
commit
33403dedea
@ -1,9 +1,10 @@
|
||||
/*!
|
||||
* \file volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn.h
|
||||
* \brief VOLK_GNSSSDR kernel: multiplies N complex (32-bit float per component) vectors
|
||||
* by a common vector, phase rotated and accumulates the results in N float complex outputs.
|
||||
* by a common vector, phase rotated with Doppler rate and accumulates the results in N float complex outputs.
|
||||
* \authors <ul>
|
||||
* <li> Antonio Ramos 2018. antonio.ramosdet(at)gmail.com
|
||||
* <li> Carles Fernandez, 2019 cfernandez@cttc.es
|
||||
* <li> Javier Arribas, 2019 javiarribas@cttc.es
|
||||
* </ul>
|
||||
*
|
||||
* VOLK_GNSSSDR kernel that multiplies N 32 bits complex vectors by a common vector, which is
|
||||
@ -43,8 +44,8 @@
|
||||
*
|
||||
* Rotates and multiplies the reference complex vector with an arbitrary number of other real vectors,
|
||||
* accumulates the results and stores them in the output vector.
|
||||
* The rotation is done at a fixed rate per sample, from an initial \p phase offset.
|
||||
* This function can be used for Doppler wipe-off and multiple correlator.
|
||||
* The rotation is done at a variable rate per sample, from an initial \p phase offset.
|
||||
* This function can be used for Doppler wipe-off and multiple correlator in the presence of Doppler rate.
|
||||
*
|
||||
* <b>Dispatcher Prototype</b>
|
||||
* \code
|
||||
@ -70,24 +71,19 @@
|
||||
#define INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_H
|
||||
|
||||
|
||||
#include <volk_gnsssdr/saturation_arithmetic.h>
|
||||
#include <volk_gnsssdr/volk_gnsssdr.h>
|
||||
#include <volk_gnsssdr/volk_gnsssdr_complex.h>
|
||||
#include <volk_gnsssdr/volk_gnsssdr_malloc.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);
|
||||
lv_32fc_t phase_doppler_rate = lv_cmake(1.0f, 0.0f);
|
||||
lv_32fc_t phase_doppler = (*phase);
|
||||
int n_vec;
|
||||
unsigned int n;
|
||||
for (n_vec = 0; n_vec < num_a_vectors; n_vec++)
|
||||
@ -96,27 +92,60 @@ static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_g
|
||||
}
|
||||
for (n = 0; n < num_points; n++)
|
||||
{
|
||||
tmp32_1 = *in_common++ * (*phase);
|
||||
// Regenerate phase
|
||||
if (n % 256 == 0)
|
||||
{
|
||||
#ifdef __cplusplus
|
||||
(*phase) /= std::abs((*phase));
|
||||
delta_phase_rate /= std::abs(delta_phase_rate);
|
||||
#else
|
||||
(*phase) /= hypotf(lv_creal(*phase), lv_cimag(*phase));
|
||||
delta_phase_rate /= hypotf(lv_creal(delta_phase_rate), lv_cimag(delta_phase_rate));
|
||||
#endif
|
||||
}
|
||||
(*phase) *= (constant_rotation * delta_phase_rate);
|
||||
delta_phase_rate *= phase_inc_rate;
|
||||
tmp32_1 = *in_common++ * (*phase);
|
||||
|
||||
phase_doppler *= phase_inc;
|
||||
phase_doppler_rate = cpowf(phase_inc_rate, lv_cmake(n * n, 0.0f));
|
||||
(*phase) = phase_doppler * phase_doppler_rate;
|
||||
|
||||
for (n_vec = 0; n_vec < num_a_vectors; n_vec++)
|
||||
{
|
||||
result[n_vec] += (tmp32_1 * in_a[n_vec][n]);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /*LV_HAVE_GENERIC*/
|
||||
|
||||
#ifdef LV_HAVE_GENERIC
|
||||
static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_generic_acc(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, const lv_32fc_t phase_inc_rate, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points)
|
||||
{
|
||||
lv_32fc_t tmp32_1 = lv_cmake(0.0f, 0.0f);
|
||||
lv_32fc_t phase_rate_acc = lv_cmake(1.0f, 0.0f);
|
||||
int n_vec;
|
||||
unsigned int n;
|
||||
for (n_vec = 0; n_vec < num_a_vectors; n_vec++)
|
||||
{
|
||||
result[n_vec] = lv_cmake(0.0f, 0.0f);
|
||||
}
|
||||
for (n = 0; n < num_points; n++)
|
||||
{
|
||||
// Regenerate phase
|
||||
if (n % 256 == 0)
|
||||
{
|
||||
#ifdef __cplusplus
|
||||
(*phase) /= std::abs((*phase));
|
||||
#else
|
||||
(*phase) /= hypotf(lv_creal(*phase), lv_cimag(*phase));
|
||||
#endif
|
||||
}
|
||||
phase_rate_acc += phase_inc_rate;
|
||||
(*phase) *= lv_cmake(cosf(phase_rate_acc), sinf(phase_rate_acc));
|
||||
for (n_vec = 0; n_vec < num_a_vectors; n_vec++)
|
||||
{
|
||||
result[n_vec] += (tmp32_1 * in_a[n_vec][n]);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_H */
|
||||
|
@ -1,6 +1,7 @@
|
||||
/*!
|
||||
* \file volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc.h
|
||||
* \brief Volk puppet for the multiple 16-bit complex dot product kernel.
|
||||
* \brief VOLK_GNSSSDR kernel: multiplies N complex (32-bit float per component) vectors
|
||||
* by a common vector, phase rotated with Doppler rate and accumulates the results in N float complex outputs.
|
||||
* \authors <ul>
|
||||
* <li> Carles Fernandez Prades 2016 cfernandez at cttc dot cat
|
||||
* </ul>
|
||||
@ -61,6 +62,7 @@ static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32
|
||||
in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
||||
memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points);
|
||||
}
|
||||
|
||||
volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_generic(result, local_code, phase_inc[0], phase_inc_rate[0], phase, (const float**)in_a, num_a_vectors, num_points);
|
||||
|
||||
for (n = 0; n < num_a_vectors; n++)
|
||||
@ -71,93 +73,35 @@ static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32
|
||||
}
|
||||
#endif // Generic
|
||||
|
||||
//
|
||||
//#ifdef LV_HAVE_GENERIC
|
||||
//static inline void volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc_generic_reload(lv_32fc_t* result, const lv_32fc_t* local_code, const float* in, unsigned int num_points)
|
||||
//{
|
||||
// // phases must be normalized. Phase rotator expects a complex exponential input!
|
||||
// float rem_carrier_phase_in_rad = 0.25;
|
||||
// float phase_step_rad = 0.1;
|
||||
// lv_32fc_t phase[1];
|
||||
// phase[0] = lv_cmake(cos(rem_carrier_phase_in_rad), sin(rem_carrier_phase_in_rad));
|
||||
// lv_32fc_t phase_inc[1];
|
||||
// phase_inc[0] = lv_cmake(cos(phase_step_rad), sin(phase_step_rad));
|
||||
// int n;
|
||||
// int num_a_vectors = 3;
|
||||
// float** in_a = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_a_vectors, volk_gnsssdr_get_alignment());
|
||||
// for (n = 0; n < num_a_vectors; n++)
|
||||
// {
|
||||
// in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
||||
// memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points);
|
||||
// }
|
||||
// volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_generic_reload(result, local_code, phase_inc[0], phase, (const float**)in_a, num_a_vectors, num_points);
|
||||
//
|
||||
// for (n = 0; n < num_a_vectors; n++)
|
||||
// {
|
||||
// volk_gnsssdr_free(in_a[n]);
|
||||
// }
|
||||
// volk_gnsssdr_free(in_a);
|
||||
//}
|
||||
//
|
||||
//#endif // Generic
|
||||
//
|
||||
//#ifdef LV_HAVE_AVX
|
||||
//static inline void volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc_u_avx(lv_32fc_t* result, const lv_32fc_t* local_code, const float* in, unsigned int num_points)
|
||||
//{
|
||||
// // phases must be normalized. Phase rotator expects a complex exponential input!
|
||||
// float rem_carrier_phase_in_rad = 0.25;
|
||||
// float phase_step_rad = 0.1;
|
||||
// lv_32fc_t phase[1];
|
||||
// phase[0] = lv_cmake(cos(rem_carrier_phase_in_rad), sin(rem_carrier_phase_in_rad));
|
||||
// lv_32fc_t phase_inc[1];
|
||||
// phase_inc[0] = lv_cmake(cos(phase_step_rad), sin(phase_step_rad));
|
||||
// int n;
|
||||
// int num_a_vectors = 3;
|
||||
// float** in_a = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_a_vectors, volk_gnsssdr_get_alignment());
|
||||
// for (n = 0; n < num_a_vectors; n++)
|
||||
// {
|
||||
// in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
||||
// memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points);
|
||||
// }
|
||||
// volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_u_avx(result, local_code, phase_inc[0], phase, (const float**)in_a, num_a_vectors, num_points);
|
||||
//
|
||||
// for (n = 0; n < num_a_vectors; n++)
|
||||
// {
|
||||
// volk_gnsssdr_free(in_a[n]);
|
||||
// }
|
||||
// volk_gnsssdr_free(in_a);
|
||||
//}
|
||||
//
|
||||
//#endif // AVX
|
||||
//
|
||||
//
|
||||
//#ifdef LV_HAVE_AVX
|
||||
//static inline void volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc_a_avx(lv_32fc_t* result, const lv_32fc_t* local_code, const float* in, unsigned int num_points)
|
||||
//{
|
||||
// // phases must be normalized. Phase rotator expects a complex exponential input!
|
||||
// float rem_carrier_phase_in_rad = 0.25;
|
||||
// float phase_step_rad = 0.1;
|
||||
// lv_32fc_t phase[1];
|
||||
// phase[0] = lv_cmake(cos(rem_carrier_phase_in_rad), sin(rem_carrier_phase_in_rad));
|
||||
// lv_32fc_t phase_inc[1];
|
||||
// phase_inc[0] = lv_cmake(cos(phase_step_rad), sin(phase_step_rad));
|
||||
// int n;
|
||||
// int num_a_vectors = 3;
|
||||
// float** in_a = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_a_vectors, volk_gnsssdr_get_alignment());
|
||||
// for (n = 0; n < num_a_vectors; n++)
|
||||
// {
|
||||
// in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
||||
// memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points);
|
||||
// }
|
||||
// volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_a_avx(result, local_code, phase_inc[0], phase, (const float**)in_a, num_a_vectors, num_points);
|
||||
//
|
||||
// for (n = 0; n < num_a_vectors; n++)
|
||||
// {
|
||||
// volk_gnsssdr_free(in_a[n]);
|
||||
// }
|
||||
// volk_gnsssdr_free(in_a);
|
||||
//}
|
||||
//
|
||||
//#endif // AVX
|
||||
#ifdef LV_HAVE_GENERIC
|
||||
static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc_generic_acc(lv_32fc_t* result, const lv_32fc_t* local_code, const float* in, unsigned int num_points)
|
||||
{
|
||||
// phases must be normalized. Phase rotator expects a complex exponential input!
|
||||
float rem_carrier_phase_in_rad = 0.25;
|
||||
float phase_step_rad = 0.1;
|
||||
lv_32fc_t phase[1];
|
||||
phase[0] = lv_cmake(cos(rem_carrier_phase_in_rad), sin(rem_carrier_phase_in_rad));
|
||||
lv_32fc_t phase_inc[1];
|
||||
phase_inc[0] = lv_cmake(cos(phase_step_rad), sin(phase_step_rad));
|
||||
lv_32fc_t phase_inc_rate[1];
|
||||
phase_inc_rate[0] = lv_cmake(cos(phase_step_rad * 0.001), sin(phase_step_rad * 0.001));
|
||||
int n;
|
||||
int num_a_vectors = 3;
|
||||
float** in_a = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_a_vectors, volk_gnsssdr_get_alignment());
|
||||
for (n = 0; n < num_a_vectors; n++)
|
||||
{
|
||||
in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
||||
memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points);
|
||||
}
|
||||
|
||||
volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_generic_acc(result, local_code, phase_inc[0], phase_inc_rate[0], phase, (const float**)in_a, num_a_vectors, num_points);
|
||||
|
||||
for (n = 0; n < num_a_vectors; n++)
|
||||
{
|
||||
volk_gnsssdr_free(in_a[n]);
|
||||
}
|
||||
volk_gnsssdr_free(in_a);
|
||||
}
|
||||
#endif // Generic
|
||||
|
||||
#endif // INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc_H
|
||||
|
Loading…
Reference in New Issue
Block a user