[hamradio-commits] [gnss-sdr] 37/126: Code cleaning and new cpu multicorrelator library
Carles Fernandez
carles_fernandez-guest at moszumanska.debian.org
Sat Dec 26 18:37:59 UTC 2015
This is an automated email from the git hooks/post-receive script.
carles_fernandez-guest pushed a commit to branch next
in repository gnss-sdr.
commit 6b340696ed78cb31a2a1787e7835200acdf03db5
Author: Javier Arribas <javiarribas at gmail.com>
Date: Tue Nov 17 19:14:55 2015 +0100
Code cleaning and new cpu multicorrelator library
---
.../gnuradio_blocks/gps_l1_ca_observables_cc.cc | 11 +-
.../gps_l1_ca_dll_pll_artemisa_tracking_cc.cc | 259 ++++++++-------------
.../gps_l1_ca_dll_pll_artemisa_tracking_cc.h | 28 +--
src/algorithms/tracking/libs/CMakeLists.txt | 1 +
.../tracking/libs/cpu_multicorrelator.cc | 167 +++++++++++++
src/algorithms/tracking/libs/cpu_multicorrelator.h | 98 ++++++++
src/core/system_parameters/gnss_synchro.h | 4 +
7 files changed, 386 insertions(+), 182 deletions(-)
diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
index 8df2646..a02b9fc 100644
--- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
+++ b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
@@ -159,6 +159,8 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m]
// update the pseudorange object
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second;
+ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].debug_var1=delta_rx_time_ms;
+ //current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads = current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads+ GPS_TWO_PI*0.001*delta_rx_time_ms*current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000)/1000 + GPS_STARTOFFSET_ms/1000.0;
@@ -175,11 +177,16 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
{
tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol;
d_dump_file.write((char*)&tmp_double, sizeof(double));
- tmp_double = current_gnss_synchro[i].Prn_timestamp_ms;
+ //tmp_double = current_gnss_synchro[i].Prn_timestamp_ms;
+ tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz;
+ d_dump_file.write((char*)&tmp_double, sizeof(double));
+ tmp_double = current_gnss_synchro[i].Carrier_phase_rads/GPS_TWO_PI;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
- tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true);
+ //tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true);
+ //tmp_double = current_gnss_synchro[i].debug_var1;
+ tmp_double= current_gnss_synchro[i].debug_var2;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].PRN;
d_dump_file.write((char*)&tmp_double, sizeof(double));
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc
index 292747c..f84c344 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc
@@ -35,7 +35,7 @@
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <gnuradio/io_signature.h>
-#include <gnuradio/fxpt.h> // fixed point sine and cosine
+#include <volk/volk.h>
#include <glog/logging.h>
#include "gnss_synchro.h"
#include "gps_sdr_signal_processing.h"
@@ -102,6 +102,7 @@ gps_l1_ca_dll_pll_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_tracking_cc(
d_fs_in = fs_in;
d_vector_length = vector_length;
d_dump_filename = dump_filename;
+ d_current_prn_length_samples = static_cast<int>(d_vector_length);
// Initialize tracking ==========================================
d_code_loop_filter.set_DLL_BW(dll_bw_hz);
@@ -112,21 +113,22 @@ gps_l1_ca_dll_pll_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_tracking_cc(
// Initialization of local code replica
// Get space for a vector with the C/A code replica sampled 1x/chip
- d_ca_code = static_cast<gr_complex*>(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS + 2) * sizeof(gr_complex), volk_get_alignment()));
-
- // Get space for the resampled early / prompt / late local replicas
- d_early_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
- d_prompt_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
- d_late_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
-
- // space for carrier wipeoff and signal baseband vectors
- d_carr_sign = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
+ d_ca_code = static_cast<gr_complex*>(volk_malloc(static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_get_alignment()));
// correlator outputs (scalar)
- d_Early = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
- d_Prompt = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
- d_Late = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
-
+ d_n_correlator_taps=3; // Early, Prompt, and Late
+ d_correlator_outs = static_cast<gr_complex*>(volk_malloc(d_n_correlator_taps*sizeof(gr_complex), volk_get_alignment()));
+ for (int n=0;n<d_n_correlator_taps;n++)
+ {
+ d_correlator_outs[n] = gr_complex(0,0);
+ }
+ d_local_code_shift_chips = static_cast<float*>(volk_malloc(d_n_correlator_taps*sizeof(float), volk_get_alignment()));
+ // Set TAPs delay values [chips]
+ d_local_code_shift_chips[0]=-d_early_late_spc_chips;
+ d_local_code_shift_chips[1]=0.0;
+ d_local_code_shift_chips[2]=d_early_late_spc_chips;
+
+ multicorrelator_cpu.init(2*d_current_prn_length_samples,d_n_correlator_taps);
//--- Perform initializations ------------------------------
// define initial code frequency basis of NCO
@@ -134,7 +136,7 @@ gps_l1_ca_dll_pll_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_tracking_cc(
// define residual code phase (in chips)
d_rem_code_phase_samples = 0.0;
// define residual carrier phase
- d_rem_carr_phase_rad = 0.0;
+ d_rem_carrier_phase_rad = 0.0;
// sample synchronization
d_sample_counter = 0;
@@ -145,8 +147,6 @@ gps_l1_ca_dll_pll_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_tracking_cc(
d_pull_in = false;
d_last_seg = 0;
- d_current_prn_length_samples = static_cast<int>(d_vector_length);
-
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES];
@@ -170,13 +170,14 @@ gps_l1_ca_dll_pll_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_tracking_cc(
d_acc_carrier_phase_rad = 0.0;
d_code_phase_samples = 0.0;
- d_pll_to_dll_assist_secs_ti=0.0;
+ d_pll_to_dll_assist_secs_Ti=0.0;
//set_min_output_buffer((long int)300);
}
void gps_l1_ca_dll_pll_artemisa_tracking_cc::start_tracking()
{
+
/*
* correct the code phase according to the delay between acq and trk
*/
@@ -197,6 +198,7 @@ void gps_l1_ca_dll_pll_artemisa_tracking_cc::start_tracking()
float T_prn_mod_seconds;
float T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
+ d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1/d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<float>(d_fs_in);
@@ -218,20 +220,28 @@ void gps_l1_ca_dll_pll_artemisa_tracking_cc::start_tracking()
d_acq_code_phase_samples = corrected_acq_phase_samples;
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
+ d_carrier_phase_step_rad=GPS_TWO_PI*d_carrier_doppler_hz/static_cast<float>(d_fs_in);
// DLL/PLL filter initialization
- d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz);
+ d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); //The carrier loop filter implements the Doppler accumulator
d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
- gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0);
- d_ca_code[0] = d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS)];
- d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) + 1] = d_ca_code[1];
+ gps_l1_ca_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN, 0);
+
+
+ multicorrelator_cpu.set_local_code_and_taps(static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS),d_ca_code,d_local_code_shift_chips);
+ for (int n=0;n<d_n_correlator_taps;n++)
+ {
+ d_correlator_outs[n] = gr_complex(0,0);
+ }
d_carrier_lock_fail_counter = 0;
- d_rem_code_phase_samples = 0;
- d_rem_carr_phase_rad = 0;
- d_acc_carrier_phase_rad = 0;
+ d_rem_code_phase_samples = 0.0;
+ d_rem_carrier_phase_rad = 0.0;
+ d_rem_code_phase_chips =0.0;
+ d_acc_carrier_phase_rad = 0.0;
+ d_pll_to_dll_assist_secs_Ti=0.0;
d_code_phase_samples = d_acq_code_phase_samples;
@@ -247,95 +257,22 @@ void gps_l1_ca_dll_pll_artemisa_tracking_cc::start_tracking()
d_pull_in = true;
d_enable_tracking = true;
- d_pll_to_dll_assist_secs_ti=0.0;
-
LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
<< " Code Phase correction [samples]=" << delay_correction_samples
<< " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples;
}
-
-// LOCAL CODE INPUT COMMANDS:
-// - double d_code_freq_chips: GPS L1 CA code frequency estimation [chips/s]
-// - double d_fs_in: sampling frequency [Hz]
-// - double d_rem_code_phase_samples: initial code phase [samples]
-// - double d_early_late_spc_chips: Early and Late replicas spacing in chips
-// - int d_current_prn_length_samples: number of code replica samples to generate
-// - gr_complex* d_ca_code: vector with GPS CA code (1 sample per chip)
-// NCO OUTPUT:
-// - gr_complex d_early_code[d_current_prn_length_samples]
-// - gr_complex d_prompt_code[d_current_prn_length_samples]
-// - gr_complex d_late_code[d_current_prn_length_samples]
-
-void gps_l1_ca_dll_pll_artemisa_tracking_cc::update_local_code()
-{
- double tcode_chips;
- double rem_code_phase_chips;
- int associated_chip_index;
- int code_length_chips = static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS);
- double code_phase_step_chips;
- int early_late_spc_samples;
- int epl_loop_length_samples;
-
- // unified loop for E, P, L code vectors
- code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
- rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in);
- tcode_chips = -rem_code_phase_chips;
-
- // Alternative EPL code generation (40% of speed improvement!)
- early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips);
- epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples * 2;
- for (int i = 0; i < epl_loop_length_samples; i++)
- {
- associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips));
- d_early_code[i] = d_ca_code[associated_chip_index];
- tcode_chips = tcode_chips + code_phase_step_chips;
- }
-
- memcpy(d_prompt_code, &d_early_code[early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex));
- memcpy(d_late_code, &d_early_code[early_late_spc_samples * 2], d_current_prn_length_samples * sizeof(gr_complex));
-}
-
-
-
-// NCO INPUT COMMANDS:
-// - double d_rem_carr_phase_rad: initial phase [rad]
-// - double d_carrier_doppler_hz: nco frequency [Hz]
-// - int d_current_prn_length_samples: number of carrier replica samples to generate
-// NCO OUTPUT:
-// - gr_complex d_carr_sign[d_current_prn_length_samples]: carrier signal cpx samples vector
-
-void gps_l1_ca_dll_pll_artemisa_tracking_cc::update_local_carrier()
-{
- float sin_f, cos_f;
- float phase_step_rad = static_cast<float>(GPS_TWO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
- int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad);
- int phase_rad_i = gr::fxpt::float_to_fixed(d_rem_carr_phase_rad);
-
- for(int i = 0; i < d_current_prn_length_samples; i++)
- {
- gr::fxpt::sincos(phase_rad_i, &sin_f, &cos_f);
- d_carr_sign[i] = std::complex<float>(cos_f, -sin_f);
- phase_rad_i += phase_step_rad_i;
- }
-}
-
-
gps_l1_ca_dll_pll_artemisa_tracking_cc::~gps_l1_ca_dll_pll_artemisa_tracking_cc()
{
d_dump_file.close();
- volk_free(d_prompt_code);
- volk_free(d_late_code);
- volk_free(d_early_code);
- volk_free(d_carr_sign);
- volk_free(d_Early);
- volk_free(d_Prompt);
- volk_free(d_Late);
+ volk_free(d_local_code_shift_chips);
+ volk_free(d_correlator_outs);
volk_free(d_ca_code);
delete[] d_Prompt_buffer;
+ multicorrelator_cpu.free();
}
@@ -351,17 +288,16 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_
Gnss_Synchro current_synchro_data = Gnss_Synchro();
// process vars
- float code_error_chips=0.0;
- float code_error_secs=0.0;
+ float code_error_chips_Ti=0.0;
float code_error_filt_chips=0.0;
- float code_error_filt_secs=0.0;
+ float code_error_filt_secs_Ti=0.0;
float INTEGRATION_TIME=0.0;
INTEGRATION_TIME=GPS_L1_CA_CODE_PERIOD; // [Ti]
- float dll_delta_rho=0.0;
- float carr_phase_error_secs_ti=0.0;
+ float dll_code_error_secs_Ti=0.0;
+ float carr_phase_error_secs_Ti=0.0;
float carr_phase_error_filt_secs_ti=0.0;
- float pll_to_dll_assist_secs_ti=0.0;
-
+ double old_d_rem_code_phase_samples;
+ double old_d_acc_carrier_phase_rad;
if (d_enable_tracking == true)
{
// Receiver signal alignment
@@ -385,69 +321,65 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_
// Fill the acquisition data
current_synchro_data = *d_acquisition_gnss_synchro;
- // ################# CARRIER NCO AND LOCAL REPLICA GENERATION ################
- update_local_code();
- update_local_carrier();
+ multicorrelator_cpu.set_input_output_vectors(d_correlator_outs,in);
// ################# perform carrier wipe-off and compute Early, Prompt and Late correlation ################
- d_correlator.Carrier_wipeoff_and_EPL_volk(d_current_prn_length_samples,
- in,
- d_carr_sign,
- d_early_code,
- d_prompt_code,
- d_late_code,
- d_Early,
- d_Prompt,
- d_Late);
+ multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carrier_phase_rad,d_carrier_phase_step_rad,d_rem_code_phase_chips,d_code_phase_step_chips,d_current_prn_length_samples);
// ################## DLL ##########################################################
// DLL discriminator
- code_error_chips = dll_nc_e_minus_l_normalized(*d_Early, *d_Late); //[chips/Ti]
- code_error_secs = code_error_chips*GPS_L1_CA_CHIP_PERIOD;
+ code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); //[chips/Ti] //early and late
// Code discriminator filter
- code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
- code_error_filt_secs = code_error_filt_chips*GPS_L1_CA_CHIP_PERIOD*GPS_L1_CA_CODE_PERIOD;
+ code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips_Ti); //input [chips/Ti] -> output [chips/second]
+ code_error_filt_secs_Ti = code_error_filt_chips*GPS_L1_CA_CHIP_PERIOD*GPS_L1_CA_CODE_PERIOD; // [s/Ti]
// DLL code error estimation [s/Ti]
- dll_delta_rho=-code_error_filt_secs+d_pll_to_dll_assist_secs_ti;
+ dll_code_error_secs_Ti=-code_error_filt_secs_Ti+d_pll_to_dll_assist_secs_Ti;
// ################## PLL ##########################################################
// PLL discriminator [rads/Ti -> Secs/Ti]
- carr_phase_error_secs_ti = pll_cloop_two_quadrant_atan(*d_Prompt)/GPS_TWO_PI;
+ carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1])/GPS_TWO_PI; //prompt output
// Carrier discriminator filter
+ // NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
//d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_phase_error_filt_secs_ti/INTEGRATION_TIME;
- d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, carr_phase_error_secs_ti, INTEGRATION_TIME);
- // PLL to DLL assistance [Secs/Ti]
- pll_to_dll_assist_secs_ti = d_carrier_doppler_hz*GPS_L1_CA_CODE_PERIOD;
- d_pll_to_dll_assist_secs_ti = pll_to_dll_assist_secs_ti/GPS_L1_FREQ_HZ;
- // New carrier Doppler frequency estimation
- //PLL COMMAND
- d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;//GPS_TWO_PI*carr_phase_error_filt_secs_ti;
- // New code Doppler frequency estimation
- d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ;// + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
+ // Input [s/Ti] -> output [Hz]
+ d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, carr_phase_error_secs_Ti, INTEGRATION_TIME);
//carrier phase accumulator for (K) doppler estimation
- d_acc_carrier_phase_rad += GPS_TWO_PI*d_carrier_doppler_hz*INTEGRATION_TIME;
+ //d_acc_carrier_phase_rad -= (GPS_TWO_PI*d_carrier_doppler_hz*INTEGRATION_TIME);
+ old_d_acc_carrier_phase_rad=d_acc_carrier_phase_rad;
+ d_acc_carrier_phase_rad -= (GPS_TWO_PI*static_cast<double>(d_carrier_doppler_hz)*static_cast<double>(INTEGRATION_TIME));
+ // PLL to DLL assistance [Secs/Ti]
+ d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz*GPS_L1_CA_CODE_PERIOD)/GPS_L1_FREQ_HZ;
+ // code frequency (include code Doppler estimation here)
+ d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ;
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
- //DLL COMMAND
// keep alignment parameters for the next input buffer
- double T_chip_seconds;
- double T_prn_seconds;
double T_prn_samples;
double K_blk_samples;
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
- T_chip_seconds = 1 / static_cast<double>(d_code_freq_chips);
- T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
- T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
- K_blk_samples = T_prn_samples + d_rem_code_phase_samples - static_cast<double>(dll_delta_rho) * static_cast<double>(d_fs_in);
+ T_prn_samples = GPS_L1_CA_CODE_PERIOD * static_cast<double>(d_fs_in);
+ K_blk_samples = T_prn_samples + d_rem_code_phase_samples - static_cast<double>(dll_code_error_secs_Ti) * static_cast<double>(d_fs_in);
d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples
-
+ old_d_rem_code_phase_samples=d_rem_code_phase_samples;
d_rem_code_phase_samples = K_blk_samples - static_cast<double>(d_current_prn_length_samples); //rounding error < 1 sample
+ //################### PLL COMMANDS #################################################
+ //carrier phase step (NCO phase increment per sample) [rads/sample]
+ d_carrier_phase_step_rad=GPS_TWO_PI*d_carrier_doppler_hz/static_cast<float>(d_fs_in);
+ //remnant carrier phase [rad]
+ d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD,GPS_TWO_PI);//GPS_TWO_PI*carr_phase_error_filt_secs_ti;
+
+ //################### DLL COMMANDS #################################################
+ //code phase step (Code resampler phase increment per sample) [chips/sample]
+ d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
+ //remnant code phase [chips]
+ d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in));
+
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
- d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt;
+ d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt
d_cn0_estimation_counter++;
}
else
@@ -482,11 +414,11 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_
// ########### Output the tracking data to navigation and PVT ##########
- current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
- current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
+ current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());
+ current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??)
- current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
+ current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
@@ -541,9 +473,10 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_
std::cout << tmp_str_stream.rdbuf() << std::flush;
}
}
- *d_Early = gr_complex(0,0);
- *d_Prompt = gr_complex(0,0);
- *d_Late = gr_complex(0,0);
+ for (int n=0;n<d_n_correlator_taps;n++)
+ {
+ d_correlator_outs[n] = gr_complex(0,0);
+ }
current_synchro_data.System = {'G'};
current_synchro_data.Flag_valid_pseudorange = false;
@@ -558,11 +491,11 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_
float tmp_E, tmp_P, tmp_L;
float tmp_float;
double tmp_double;
- prompt_I = (*d_Prompt).real();
- prompt_Q = (*d_Prompt).imag();
- tmp_E = std::abs<float>(*d_Early);
- tmp_P = std::abs<float>(*d_Prompt);
- tmp_L = std::abs<float>(*d_Late);
+ prompt_I = d_correlator_outs[1].real();
+ prompt_Q = d_correlator_outs[1].imag();
+ tmp_E = std::abs<float>(d_correlator_outs[0]);
+ tmp_P = std::abs<float>(d_correlator_outs[1]);
+ tmp_L = std::abs<float>(d_correlator_outs[2]);
try
{
// EPR
@@ -584,11 +517,11 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_
d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
//PLL commands
- d_dump_file.write(reinterpret_cast<char*>(&carr_phase_error_secs_ti), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&carr_phase_error_filt_secs_ti), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&carr_phase_error_secs_Ti), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
//DLL commands
- d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&code_error_chips_Ti), sizeof(float));
d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
// CN0 and carrier lock test
@@ -601,9 +534,9 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_
tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
}
- catch (std::ifstream::failure e)
+ catch (const std::ifstream::failure* e)
{
- LOG(WARNING) << "Exception writing trk dump file " << e.what();
+ LOG(WARNING) << "Exception writing trk dump file " << e->what();
}
}
@@ -632,9 +565,9 @@ void gps_l1_ca_dll_pll_artemisa_tracking_cc::set_channel(unsigned int channel)
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl;
}
- catch (std::ifstream::failure e)
+ catch (const std::ifstream::failure* e)
{
- LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what() << std::endl;
+ LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e->what() << std::endl;
}
}
}
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h
index b3ddf80..26ed6a0 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h
@@ -50,7 +50,7 @@
#include "gnss_synchro.h"
#include "tracking_2nd_DLL_filter.h"
#include "tracking_FLL_PLL_filter.h"
-#include "correlator.h"
+#include "cpu_multicorrelator.h"
class gps_l1_ca_dll_pll_artemisa_tracking_cc;
@@ -109,8 +109,6 @@ private:
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips);
- void update_local_code();
- void update_local_carrier();
// tracking configuration vars
boost::shared_ptr<gr::msg_queue> d_queue;
@@ -125,21 +123,17 @@ private:
long d_fs_in;
double d_early_late_spc_chips;
+ int d_n_correlator_taps;
gr_complex* d_ca_code;
-
- gr_complex* d_early_code;
- gr_complex* d_late_code;
- gr_complex* d_prompt_code;
- gr_complex* d_carr_sign;
-
- gr_complex *d_Early;
- gr_complex *d_Prompt;
- gr_complex *d_Late;
+ float* d_local_code_shift_chips;
+ gr_complex* d_correlator_outs;
+ cpu_multicorrelator multicorrelator_cpu;
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
- float d_rem_carr_phase_rad;
+ float d_rem_code_phase_chips;
+ float d_rem_carrier_phase_rad;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
@@ -148,15 +142,15 @@ private:
// acquisition
float d_acq_code_phase_samples;
float d_acq_carrier_doppler_hz;
- // correlator
- Correlator d_correlator;
// tracking vars
double d_code_freq_chips;
+ float d_code_phase_step_chips;
float d_carrier_doppler_hz;
- float d_acc_carrier_phase_rad;
+ float d_carrier_phase_step_rad;
+ double d_acc_carrier_phase_rad;
float d_code_phase_samples;
- float d_pll_to_dll_assist_secs_ti;
+ float d_pll_to_dll_assist_secs_Ti;
//PRN period in samples
int d_current_prn_length_samples;
diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt
index 7d0fb9d..cc53a91 100644
--- a/src/algorithms/tracking/libs/CMakeLists.txt
+++ b/src/algorithms/tracking/libs/CMakeLists.txt
@@ -38,6 +38,7 @@ endif(ENABLE_CUDA)
set(TRACKING_LIB_SOURCES
correlator.cc
+ cpu_multicorrelator.cc
lock_detectors.cc
tcp_communication.cc
tcp_packet_data.cc
diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator.cc b/src/algorithms/tracking/libs/cpu_multicorrelator.cc
new file mode 100644
index 0000000..0ed2ac9
--- /dev/null
+++ b/src/algorithms/tracking/libs/cpu_multicorrelator.cc
@@ -0,0 +1,167 @@
+/*!
+ * \file cpu_multicorrelator.cc
+ * \brief High optimized CPU vector multiTAP correlator class
+ * \authors <ul>
+ * <li> Javier Arribas, 2015. jarribas(at)cttc.es
+ * </ul>
+ *
+ * Class that implements a high optimized vector multiTAP correlator class for CPUs
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015 (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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "cpu_multicorrelator.h"
+
+#include <iostream>
+#include <volk/volk.h>
+#include <gnuradio/fxpt.h> // fixed point sine and cosine
+#include <cmath>
+
+bool cpu_multicorrelator::init(
+ int max_signal_length_samples,
+ int n_correlators
+ )
+{
+
+ // ALLOCATE MEMORY FOR INTERNAL vectors
+ size_t size = max_signal_length_samples * sizeof(std::complex<float>);
+
+ // NCO signal
+ d_nco_in=static_cast<std::complex<float>*>(volk_malloc(size, volk_get_alignment()));
+
+ // Doppler-free signal
+ d_sig_doppler_wiped=static_cast<std::complex<float>*>(volk_malloc(size, volk_get_alignment()));
+
+ d_local_codes_resampled=new std::complex<float>*[n_correlators];
+ for (int n=0;n<n_correlators;n++)
+ {
+ d_local_codes_resampled[n]=static_cast<std::complex<float>*>(volk_malloc(size, volk_get_alignment()));
+ }
+ d_n_correlators=n_correlators;
+ return true;
+}
+
+bool cpu_multicorrelator::set_local_code_and_taps(
+ int code_length_chips,
+ const std::complex<float>* local_code_in,
+ float *shifts_chips
+ )
+{
+
+ d_local_code_in=local_code_in;
+ d_shifts_chips=shifts_chips;
+ d_code_length_chips=code_length_chips;
+ return true;
+}
+
+bool cpu_multicorrelator::set_input_output_vectors(
+ std::complex<float>* corr_out,
+ const std::complex<float>* sig_in
+ )
+{
+ // Save CPU pointers
+ d_sig_in =sig_in;
+ d_corr_out = corr_out;
+ return true;
+
+}
+
+void cpu_multicorrelator::update_local_code(int correlator_length_samples,float rem_code_phase_chips, float code_phase_step_chips)
+{
+ float local_code_chip_index;
+ for (int current_correlator_tap=0; current_correlator_tap<d_n_correlators;current_correlator_tap++)
+ {
+ for (int n = 0; n < correlator_length_samples; n++)
+ {
+ // resample code for current tap
+ local_code_chip_index= fmod(code_phase_step_chips*static_cast<float>(n)+ d_shifts_chips[current_correlator_tap] - rem_code_phase_chips, d_code_length_chips);
+ //Take into account that in multitap correlators, the shifts can be negative!
+ if (local_code_chip_index<0.0) local_code_chip_index+=d_code_length_chips;
+ d_local_codes_resampled[current_correlator_tap][n]=d_local_code_in[static_cast<int>(round(local_code_chip_index))];
+
+ }
+ }
+}
+
+
+void cpu_multicorrelator::update_local_carrier(int correlator_length_samples, float rem_carr_phase_rad, float phase_step_rad)
+{
+ float sin_f, cos_f;
+ int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad);
+ int phase_rad_i = gr::fxpt::float_to_fixed(rem_carr_phase_rad);
+
+ for(int i = 0; i < correlator_length_samples; i++)
+ {
+ gr::fxpt::sincos(phase_rad_i, &sin_f, &cos_f);
+ d_nco_in[i] = std::complex<float>(cos_f, -sin_f);
+ phase_rad_i += phase_step_rad_i;
+ }
+}
+
+bool cpu_multicorrelator::Carrier_wipeoff_multicorrelator_resampler(
+ float rem_carrier_phase_in_rad,
+ float phase_step_rad,
+ float rem_code_phase_chips,
+ float code_phase_step_chips,
+ int signal_length_samples)
+ {
+
+ update_local_carrier(signal_length_samples, rem_carrier_phase_in_rad, phase_step_rad);
+ update_local_code(signal_length_samples,rem_code_phase_chips, code_phase_step_chips);
+
+ volk_32fc_x2_multiply_32fc(d_sig_doppler_wiped, d_sig_in, d_nco_in, signal_length_samples);
+ for (int current_correlator_tap=0; current_correlator_tap<d_n_correlators;current_correlator_tap++)
+ {
+ volk_32fc_x2_dot_prod_32fc(&d_corr_out[current_correlator_tap], d_sig_doppler_wiped, d_local_codes_resampled[current_correlator_tap], signal_length_samples);
+ }
+ return true;
+}
+
+
+cpu_multicorrelator::cpu_multicorrelator()
+{
+ d_sig_in=NULL;
+ d_nco_in=NULL;
+ d_sig_doppler_wiped=NULL;
+ d_local_code_in=NULL;
+ d_shifts_chips=NULL;
+ d_corr_out=NULL;
+ d_code_length_chips=0;
+ d_n_correlators=0;
+}
+
+bool cpu_multicorrelator::free()
+{
+ // Free memory
+ if (d_sig_doppler_wiped!=NULL) volk_free(d_sig_doppler_wiped);
+ if (d_nco_in!=NULL) volk_free(d_nco_in);
+ for (int n=0;n<d_n_correlators;n++)
+ {
+ volk_free(d_local_codes_resampled[n]);
+ }
+ delete d_local_codes_resampled;
+ return true;
+}
+
diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator.h b/src/algorithms/tracking/libs/cpu_multicorrelator.h
new file mode 100644
index 0000000..ca0f068
--- /dev/null
+++ b/src/algorithms/tracking/libs/cpu_multicorrelator.h
@@ -0,0 +1,98 @@
+/*!
+ * \file cpu_multicorrelator.h
+ * \brief High optimized CPU vector multiTAP correlator class
+ * \authors <ul>
+ * <li> Javier Arribas, 2015. jarribas(at)cttc.es
+ * </ul>
+ *
+ * Class that implements a high optimized vector multiTAP correlator class for CPUs
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015 (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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef CPU_MULTICORRELATOR_H_
+#define CPU_MULTICORRELATOR_H_
+
+#include <complex>
+
+/*!
+ * \brief Class that implements carrier wipe-off and correlators.
+ */
+class cpu_multicorrelator
+{
+public:
+ cpu_multicorrelator();
+ bool init(
+ int max_signal_length_samples,
+ int n_correlators
+ );
+ bool set_local_code_and_taps(
+ int code_length_chips,
+ const std::complex<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
+ );
+
+ void update_local_carrier(
+ int correlator_length_samples,
+ float rem_carr_phase_rad,
+ float phase_step_rad
+ );
+ 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,
+ int signal_length_samples);
+ bool free();
+
+private:
+ // Allocate the device input vectors
+ const std::complex<float> *d_sig_in;
+ std::complex<float> *d_nco_in;
+ std::complex<float> **d_local_codes_resampled;
+ std::complex<float> *d_sig_doppler_wiped;
+ const std::complex<float> *d_local_code_in;
+ std::complex<float> *d_corr_out;
+
+ float *d_shifts_chips;
+ int d_code_length_chips;
+ int d_n_correlators;
+
+ bool update_local_code();
+ bool update_local_carrier();
+
+};
+
+
+#endif /* CPU_MULTICORRELATOR_H_ */
diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h
index df688e1..600f122 100644
--- a/src/core/system_parameters/gnss_synchro.h
+++ b/src/core/system_parameters/gnss_synchro.h
@@ -73,6 +73,10 @@ public:
// Pseudorange
double Pseudorange_m;
bool Flag_valid_pseudorange;
+
+ //debug
+ double debug_var1;
+ double debug_var2;
};
#endif
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-hamradio/gnss-sdr.git
More information about the pkg-hamradio-commits
mailing list