Fuse pilot and data correlation into a single carrier wipe-off pass

When tracking the pilot component, dll_pll_veml_tracking previously ran two separate carrier wipe-off passes over the same input samples: one for the pilot correlator taps and a second, redundant pass through a dedicated Cpu_Multicorrelator_Real_Codes instance to compute only the data prompt. The carrier rotation dominates the per-sample cost, so it was effectively being computed twice over identical data. Extend Cpu_Multicorrelator_Real_Codes with optional data-prompt support (init with has_data_prompt, set_data_code_and_prompt_tap, and Carrier_wipeoff_multicorrelator_resampler_with_data_prompt) that resamples one extra local-code buffer for the data prompt and computes all pilot taps plus the data prompt under a single carrier rotator via one VOLK_GNSSSDR xN kernel call. Scratch buffers are allocated at init, so there are no per-call heap allocations in the hot path. Both the normal and high-dynamics resampler/rotator paths are preserved, and the second correlator instance is no longer used for CPU VEML tracking. Results are bit-for-bit equivalent to the previous two-pass implementation. This cuts tracking-loop time by ~33% (about 1.5x throughput) for pilot-tracked signals (GPS L5, Galileo E1/E5a/E5b/E6, QZSS L5). Data-only tracking (track_pilot = false) is unchanged.
2026-09-15 21:51:23 +00:00 · 2026-06-30 21:37:22 +02:00
parent b898bcad28
commit fa96ce13a0
4 changed files with 151 additions and 34 deletions
@@ -650,7 +650,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_)
            d_prompt_data_shift = &d_local_code_shift_chips[1];
        }

-    d_multicorrelator_cpu.init(static_cast<int>(2 * d_trk_parameters.vector_length), d_n_correlator_taps);
+    d_multicorrelator_cpu.init(static_cast<int>(2 * d_trk_parameters.vector_length), d_n_correlator_taps, d_trk_parameters.track_pilot);

    if (d_trk_parameters.extend_correlation_symbols > 1)
        {
@@ -665,9 +665,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_)
    // Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal
    if (d_trk_parameters.track_pilot)
        {
-            // Extra correlator for the data component
-            d_correlator_data_cpu.init(static_cast<int>(2 * d_trk_parameters.vector_length), 1);
-            d_correlator_data_cpu.set_high_dynamics_resampler(d_trk_parameters.high_dyn);
            d_data_code.resize(2 * d_code_length_chips, 0.0);
        }

@@ -823,7 +820,7 @@ void dll_pll_veml_tracking::start_tracking()
                    gps_l5q_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN);
                    gps_l5i_code_gen_float(d_data_code, d_acquisition_gnss_synchro->PRN);
                    d_Prompt_Data[0] = gr_complex(0.0, 0.0);
-                    d_correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
+                    d_multicorrelator_cpu.set_data_code_and_prompt_tap(d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
                }
            else
                {
@@ -838,7 +835,7 @@ void dll_pll_veml_tracking::start_tracking()
                    galileo_e1_code_gen_sinboc11_float(d_tracking_code, pilot_signal, d_acquisition_gnss_synchro->PRN);
                    galileo_e1_code_gen_sinboc11_float(d_data_code, Signal_, d_acquisition_gnss_synchro->PRN);
                    d_Prompt_Data[0] = gr_complex(0.0, 0.0);
-                    d_correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
+                    d_multicorrelator_cpu.set_data_code_and_prompt_tap(d_code_samples_per_chip * d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
                }
            else
                {
@@ -859,7 +856,7 @@ void dll_pll_veml_tracking::start_tracking()
                            d_data_code[i] = aux_code[i].real();  // the same because it is generated the full signal (E5aI + E5aQ)
                        }
                    d_Prompt_Data[0] = gr_complex(0.0, 0.0);
-                    d_correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
+                    d_multicorrelator_cpu.set_data_code_and_prompt_tap(d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
                }
            else
                {
@@ -883,7 +880,7 @@ void dll_pll_veml_tracking::start_tracking()
                            d_data_code[i] = aux_code[i].real();  // the same because it is generated the full signal (E5bI + E5bsQ)
                        }
                    d_Prompt_Data[0] = gr_complex(0.0, 0.0);
-                    d_correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
+                    d_multicorrelator_cpu.set_data_code_and_prompt_tap(d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
                }
            else
                {
@@ -901,7 +898,7 @@ void dll_pll_veml_tracking::start_tracking()
                    galileo_e6_b_code_gen_float_primary(d_data_code, d_acquisition_gnss_synchro->PRN);
                    galileo_e6_c_code_gen_float_primary(d_tracking_code, d_acquisition_gnss_synchro->PRN);
                    d_Prompt_Data[0] = gr_complex(0.0, 0.0);
-                    d_correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
+                    d_multicorrelator_cpu.set_data_code_and_prompt_tap(d_code_samples_per_chip * d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
                }
            else
                {
@@ -1021,7 +1018,7 @@ void dll_pll_veml_tracking::start_tracking()
                    qzss_l5q_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN);
                    qzss_l5i_code_gen_float(d_data_code, d_acquisition_gnss_synchro->PRN);
                    d_Prompt_Data[0] = gr_complex(0.0, 0.0);
-                    d_correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
+                    d_multicorrelator_cpu.set_data_code_and_prompt_tap(d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
                }
            else
                {
@@ -1106,10 +1103,6 @@ dll_pll_veml_tracking::~dll_pll_veml_tracking()
        }
    try
        {
-            if (d_trk_parameters.track_pilot)
-                {
-                    d_correlator_data_cpu.free();
-                }
            d_multicorrelator_cpu.free();
        }
    catch (const std::exception &ex)
@@ -1235,20 +1228,21 @@ 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
-    d_multicorrelator_cpu.set_input_output_vectors(d_correlator_outs.data(), input_samples);
-    d_multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(
-        d_rem_carr_phase_rad,
-        static_cast<float>(d_carrier_phase_step_rad), static_cast<float>(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),
-        d_trk_parameters.vector_length);
-
-    // DATA CORRELATOR (if tracking tracks the pilot signal)
    if (d_trk_parameters.track_pilot)
        {
-            d_correlator_data_cpu.set_input_output_vectors(d_Prompt_Data.data(), input_samples);
-            d_correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler(
+            d_multicorrelator_cpu.set_input_output_vectors(d_correlator_outs.data(), d_Prompt_Data.data(), input_samples);
+            d_multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler_with_data_prompt(
+                d_rem_carr_phase_rad,
+                static_cast<float>(d_carrier_phase_step_rad), static_cast<float>(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),
+                d_trk_parameters.vector_length);
+        }
+    else
+        {
+            d_multicorrelator_cpu.set_input_output_vectors(d_correlator_outs.data(), input_samples);
+            d_multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(
                d_rem_carr_phase_rad,
                static_cast<float>(d_carrier_phase_step_rad), static_cast<float>(d_carrier_phase_rate_step_rad),
                static_cast<float>(d_rem_code_phase_chips) * static_cast<float>(d_code_samples_per_chip),
@@ -92,7 +92,6 @@ private:
    int32_t save_matfile() const;

    Cpu_Multicorrelator_Real_Codes d_multicorrelator_cpu;
-    Cpu_Multicorrelator_Real_Codes d_correlator_data_cpu;  // for data channel

    Dll_Pll_Conf d_trk_parameters;

@@ -4,6 +4,7 @@
 * \authors <ul>
 *          <li> Javier Arribas, 2015. jarribas(at)cttc.es
 *          <li> Cillian O'Driscoll, 2017. cillian.odriscoll(at)gmail.com
+ *          <li> Carles Fernandez-Prades, 2026. cfernandez(at)cttc.es
 *          </ul>
 *
 * Class that implements a highly optimized vector multiTAP correlator class for CPUs
@@ -13,7 +14,7 @@
 * GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
 * This file is part of GNSS-SDR.
 *
- * Copyright (C) 2010-2020  (see AUTHORS file for a list of contributors)
+ * Copyright (C) 2010-2026  (see AUTHORS file for a list of contributors)
 * SPDX-License-Identifier: GPL-3.0-or-later
 *
 * -----------------------------------------------------------------------------
@@ -26,7 +27,7 @@

 Cpu_Multicorrelator_Real_Codes::~Cpu_Multicorrelator_Real_Codes()
 {
-    if (d_local_codes_resampled != nullptr)
+    if (d_local_codes_resampled != nullptr || d_corr_out_with_data_prompt != nullptr)
        {
            Cpu_Multicorrelator_Real_Codes::free();
        }
@@ -36,16 +37,32 @@ Cpu_Multicorrelator_Real_Codes::~Cpu_Multicorrelator_Real_Codes()
 bool Cpu_Multicorrelator_Real_Codes::init(
    int max_signal_length_samples,
    int n_correlators)
+{
+    return init(max_signal_length_samples, n_correlators, false);
+}
+
+
+bool Cpu_Multicorrelator_Real_Codes::init(
+    int max_signal_length_samples,
+    int n_correlators,
+    bool has_data_prompt)
 {
    // ALLOCATE MEMORY FOR INTERNAL vectors
    size_t size = max_signal_length_samples * sizeof(float);
+    const int total_correlators = has_data_prompt ? n_correlators + 1 : n_correlators;

-    d_local_codes_resampled = static_cast<float**>(volk_gnsssdr_malloc(n_correlators * sizeof(float*), volk_gnsssdr_get_alignment()));
-    for (int n = 0; n < n_correlators; n++)
+    d_local_codes_resampled = static_cast<float**>(volk_gnsssdr_malloc(total_correlators * sizeof(float*), volk_gnsssdr_get_alignment()));
+    for (int n = 0; n < total_correlators; n++)
        {
            d_local_codes_resampled[n] = static_cast<float*>(volk_gnsssdr_malloc(size, volk_gnsssdr_get_alignment()));
        }
    d_n_correlators = n_correlators;
+    d_has_data_prompt = has_data_prompt;
+    d_n_correlators_with_data_prompt = total_correlators;
+    if (d_has_data_prompt)
+        {
+            d_corr_out_with_data_prompt = static_cast<std::complex<float>*>(volk_gnsssdr_malloc(total_correlators * sizeof(std::complex<float>), volk_gnsssdr_get_alignment()));
+        }
    return true;
 }

@@ -72,6 +89,29 @@ bool Cpu_Multicorrelator_Real_Codes::set_input_output_vectors(std::complex<float
 }


+bool Cpu_Multicorrelator_Real_Codes::set_data_code_and_prompt_tap(
+    int code_length_chips,
+    const float* data_code_in,
+    float* prompt_shift_chips)
+{
+    d_data_code_in = data_code_in;
+    d_data_prompt_shift_chips = prompt_shift_chips;
+    d_data_code_length_chips = code_length_chips;
+
+    return true;
+}
+
+
+bool Cpu_Multicorrelator_Real_Codes::set_input_output_vectors(std::complex<float>* corr_out, std::complex<float>* data_prompt_out, const std::complex<float>* sig_in)
+{
+    // Save CPU pointers
+    d_sig_in = sig_in;
+    d_corr_out = corr_out;
+    d_data_prompt_out = data_prompt_out;
+    return true;
+}
+
+
 void Cpu_Multicorrelator_Real_Codes::update_local_code(int correlator_length_samples, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips)
 {
    if (d_use_high_dynamics_resampler)
@@ -100,6 +140,70 @@ void Cpu_Multicorrelator_Real_Codes::update_local_code(int correlator_length_sam
 }


+bool Cpu_Multicorrelator_Real_Codes::Carrier_wipeoff_multicorrelator_resampler_with_data_prompt(
+    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)
+{
+    if (!d_has_data_prompt || d_sig_in == nullptr || d_corr_out == nullptr || d_data_code_in == nullptr || d_data_prompt_shift_chips == nullptr || d_data_prompt_out == nullptr || d_corr_out_with_data_prompt == nullptr)
+        {
+            return false;
+        }
+
+    update_local_code(signal_length_samples, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips);
+
+    float** data_prompt_resampled = &d_local_codes_resampled[d_n_correlators];
+    if (d_use_high_dynamics_resampler)
+        {
+            volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn(data_prompt_resampled,
+                d_data_code_in,
+                rem_code_phase_chips,
+                code_phase_step_chips,
+                code_phase_rate_step_chips,
+                d_data_prompt_shift_chips,
+                d_data_code_length_chips,
+                1,
+                signal_length_samples);
+        }
+    else
+        {
+            volk_gnsssdr_32f_xn_resampler_32f_xn(data_prompt_resampled,
+                d_data_code_in,
+                rem_code_phase_chips,
+                code_phase_step_chips,
+                d_data_prompt_shift_chips,
+                d_data_code_length_chips,
+                1,
+                signal_length_samples);
+        }
+
+    // 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_with_data_prompt, 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_with_data_prompt, signal_length_samples);
+        }
+    else
+        {
+            volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out_with_data_prompt, 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_with_data_prompt, signal_length_samples);
+        }
+
+    for (int n = 0; n < d_n_correlators; n++)
+        {
+            d_corr_out[n] = d_corr_out_with_data_prompt[n];
+        }
+    d_data_prompt_out[0] = d_corr_out_with_data_prompt[d_n_correlators];
+
+    return true;
+}
+
+
 bool Cpu_Multicorrelator_Real_Codes::Carrier_wipeoff_multicorrelator_resampler(
    float rem_carrier_phase_in_rad,
    float phase_step_rad,
@@ -149,13 +253,21 @@ bool Cpu_Multicorrelator_Real_Codes::free()
    // Free memory
    if (d_local_codes_resampled != nullptr)
        {
-            for (int n = 0; n < d_n_correlators; n++)
+            for (int n = 0; n < d_n_correlators_with_data_prompt; n++)
                {
                    volk_gnsssdr_free(d_local_codes_resampled[n]);
                }
            volk_gnsssdr_free(d_local_codes_resampled);
            d_local_codes_resampled = nullptr;
        }
+    if (d_corr_out_with_data_prompt != nullptr)
+        {
+            volk_gnsssdr_free(d_corr_out_with_data_prompt);
+            d_corr_out_with_data_prompt = nullptr;
+        }
+    d_n_correlators = 0;
+    d_n_correlators_with_data_prompt = 0;
+    d_has_data_prompt = false;
    return true;
 }

@@ -3,7 +3,8 @@
 * \brief Highly optimized CPU vector multiTAP correlator class using real-valued local codes
 * \authors <ul>
 *          <li> Javier Arribas, 2015. jarribas(at)cttc.es
- *          <li> Cillian O'Driscoll, 2017, cillian.odriscoll(at)gmail.com
+ *          <li> Cillian O'Driscoll, 2017. cillian.odriscoll(at)gmail.com
+ *          <li> Carles Fernandez-Prades, 2026. cfernandez(at)cttc.es
 *          </ul>
 *
 * Class that implements a highly optimized vector multiTAP correlator class for CPUs
@@ -13,7 +14,7 @@
 * GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
 * This file is part of GNSS-SDR.
 *
- * Copyright (C) 2010-2020  (see AUTHORS file for a list of contributors)
+ * Copyright (C) 2010-2026  (see AUTHORS file for a list of contributors)
 * SPDX-License-Identifier: GPL-3.0-or-later
 *
 * -----------------------------------------------------------------------------
@@ -41,22 +42,33 @@ public:
    void set_high_dynamics_resampler(bool use_high_dynamics_resampler);
    ~Cpu_Multicorrelator_Real_Codes();
    bool init(int max_signal_length_samples, int n_correlators);
+    bool init(int max_signal_length_samples, int n_correlators, bool has_data_prompt);
    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);
+    bool set_data_code_and_prompt_tap(int code_length_chips, const float *data_code_in, float *prompt_shift_chips);
+    bool set_input_output_vectors(std::complex<float> *corr_out, std::complex<float> *data_prompt_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);
    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 Carrier_wipeoff_multicorrelator_resampler_with_data_prompt(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 free();

 private:
    // Allocate the device input vectors
    const std::complex<float> *d_sig_in{nullptr};
    const float *d_local_code_in{nullptr};
+    const float *d_data_code_in{nullptr};
    std::complex<float> *d_corr_out{nullptr};
+    std::complex<float> *d_data_prompt_out{nullptr};
+    std::complex<float> *d_corr_out_with_data_prompt{nullptr};
    float **d_local_codes_resampled{nullptr};
    float *d_shifts_chips{nullptr};
+    float *d_data_prompt_shift_chips{nullptr};
    int d_code_length_chips{0};
+    int d_data_code_length_chips{0};
    int d_n_correlators{0};
+    int d_n_correlators_with_data_prompt{0};
+    bool d_has_data_prompt{false};
    bool d_use_high_dynamics_resampler{true};
 };