mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Code cleaning and new cpu multicorrelator library

This commit is contained in:
Javier Arribas 2015-11-17 19:14:55 +01:00
parent 27588fa83b
commit 6b340696ed
7 changed files with 385 additions and 181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
View File

@ -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));

257
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc
View File

@ -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;
}
}
}

28
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h
View File

@ -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;

1
src/algorithms/tracking/libs/CMakeLists.txt
View File

@ -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

167
src/algorithms/tracking/libs/cpu_multicorrelator.cc Normal file
View File

@ -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;
}

98
src/algorithms/tracking/libs/cpu_multicorrelator.h Normal file
View File

@ -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_ */

4
src/core/system_parameters/gnss_synchro.h
View File

@ -73,6 +73,10 @@ public:
// Pseudorange
double Pseudorange_m;
bool Flag_valid_pseudorange;
//debug
double debug_var1;
double debug_var2;
};
#endif