Code cleaning and few optimizations in tracking modules.

git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@148 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas 2012-01-29 18:03:56 +00:00
parent dd01b83bc3
commit 9f3fbeb52d
6 changed files with 100 additions and 159 deletions

View File

@ -17,7 +17,7 @@ ControlThread.wait_for_flowgraph=false
SignalSource.implementation=File_Signal_Source
;#filename: path to file with the captured GNSS signal samples to be processed
SignalSource.filename=/media/DATALOGGER/signals/Agilent GPS Generator/cap2/agilent_cap2.dat
SignalSource.filename=/home/javier/signals/Agilent/cap2/agilent_cap2.dat
;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
SignalSource.item_type=gr_complex
@ -226,7 +226,7 @@ Acquisition7.doppler_step=250
;######### TRACKING GLOBAL CONFIG ############
;#implementatiion: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_FLL_PLL_Tracking]
Tracking.implementation=GPS_L1_CA_DLL_FLL_PLL_Tracking
Tracking.implementation=GPS_L1_CA_DLL_PLL_Tracking
;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version.
Tracking.item_type=gr_complex

View File

@ -125,23 +125,22 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(
// Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2];
// Get space for the resampled early / prompt / late local replicas
//d_early_code = new gr_complex[d_vector_length*2];
//d_prompt_code = new gr_complex[d_vector_length*2];
//d_late_code = new gr_complex[d_vector_length*2];
// space for carrier wipeoff LO vector
//d_carr_sign = new gr_complex[d_vector_length*2];
/* If an array is partitioned for more than one thread to operate on,
* having the sub-array boundaries unaligned to cache lines could lead
* to performance degradation. Here we allocate memory
* (gr_comlex array of size 2*d_vector_length) aligned to cache of 16 bytes
*/
// todo: do something if posix_memalign fails
// Get space for the resampled early / prompt / late local replicas
if (posix_memalign((void**)&d_early_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
if (posix_memalign((void**)&d_late_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
if (posix_memalign((void**)&d_prompt_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
// space for carrier wipeoff and signal baseband vectors
if (posix_memalign((void**)&d_carr_sign, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
// correlator outputs (scalar)
if (posix_memalign((void**)&d_Early, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Prompt, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){};
// sample synchronization
d_sample_counter = 0;
@ -303,7 +302,9 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc()
free(d_late_code);
free(d_early_code);
free(d_carr_sign);
free(d_Early);
free(d_Prompt);
free(d_Late);
delete[] d_Prompt_buffer;
}
@ -322,10 +323,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
float PLL_discriminator_hz = 0;
float carr_nco_hz = 0;
d_Prompt_prev = d_Prompt; // for the FLL discriminator
d_Early = gr_complex(0,0);
d_Prompt = gr_complex(0,0);
d_Late = gr_complex(0,0);
d_Prompt_prev = *d_Prompt; // for the FLL discriminator
if (d_enable_tracking == true)
{
@ -377,15 +375,6 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
update_local_code();
update_local_carrier();
gr_complex* E_out;
gr_complex* P_out;
gr_complex* L_out;
// TODO: do something if posix_memalign fails
if (posix_memalign((void**)&E_out, 16, 8) == 0){};
if (posix_memalign((void**)&P_out, 16, 8) == 0){};
if (posix_memalign((void**)&L_out, 16, 8) == 0){};
// perform Early, Prompt and Late correlation
d_correlator.Carrier_wipeoff_and_EPL_volk(d_current_prn_length_samples,
in,
@ -393,39 +382,32 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
d_early_code,
d_prompt_code,
d_late_code,
E_out,
P_out,
L_out);
d_Early = E_out[0];
d_Prompt = P_out[0];
d_Late = L_out[0];
free(E_out);
free(P_out);
free(L_out);
d_Early,
d_Prompt,
d_Late);
/*
* DLL, FLL, and PLL discriminators
*/
// Compute DLL error
code_error_chips = dll_nc_e_minus_l_normalized(d_Early,d_Late);
code_error_chips = dll_nc_e_minus_l_normalized(*d_Early,*d_Late);
//compute FLL error
correlation_time_s = ((float)d_current_prn_length_samples) / (float)d_fs_in;
if (d_FLL_wait == 1)
{
d_Prompt_prev = d_Prompt;
d_Prompt_prev = *d_Prompt;
d_FLL_wait = 0;
}
else
{
d_FLL_discriminator_hz = fll_four_quadrant_atan(d_Prompt_prev, d_Prompt, 0, correlation_time_s) / (float)TWO_PI;
d_Prompt_prev = d_Prompt;
d_FLL_discriminator_hz = fll_four_quadrant_atan(d_Prompt_prev, *d_Prompt, 0, correlation_time_s) / (float)TWO_PI;
d_Prompt_prev = *d_Prompt;
d_FLL_wait = 1;
}
// Compute PLL error
PLL_discriminator_hz = pll_cloop_two_quadrant_atan(d_Prompt) / (float)TWO_PI;
PLL_discriminator_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / (float)TWO_PI;
/*
* \todo Update FLL assistance algorithm!
@ -448,7 +430,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
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_Prompt;
d_cn0_estimation_counter++;
}
else
@ -477,20 +459,8 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
}
// ########### Output the tracking data to navigation and PVT ##########
// // Output channel 0: Prompt correlator output Q
// *out[0] = (double)d_Prompt.real();
// // Output channel 1: Prompt correlator output I
// *out[1] = (double)d_Prompt.imag();
// // Output channel 2: PRN absolute delay [s]
// *out[2] = d_sample_counter_seconds;
// // Output channel 3: d_acc_carrier_phase_rad [rad]
// *out[3] = (double)d_acc_carrier_phase_rad;
// // Output channel 4: PRN code phase [s]
// *out[4] = (double)d_code_phase_samples * (1/(float)d_fs_in);
current_synchro_data.Prompt_I=(double)d_Prompt.real();
current_synchro_data.Prompt_Q=(double)d_Prompt.imag();
current_synchro_data.Prompt_I=(double)(*d_Prompt).real();
current_synchro_data.Prompt_Q=(double)(*d_Prompt).imag();
current_synchro_data.Tracking_timestamp_secs=d_sample_counter_seconds;
current_synchro_data.Carrier_phase_rads=(double)d_acc_carrier_phase_rad;
current_synchro_data.Code_phase_secs=(double)d_code_phase_samples * (1/(float)d_fs_in);
@ -549,12 +519,9 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
}
else
{
// double **out = (double **) &output_items[0]; //block output streams pointer
// *out[0] = 0;
// *out[1] = 0;
// *out[2] = 0;
// *out[3] = 0;
// *out[4] = 0;
*d_Early = gr_complex(0,0);
*d_Prompt = gr_complex(0,0);
*d_Late = gr_complex(0,0);
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //block output streams pointer
*out[0]=*d_acquisition_gnss_synchro;
}
@ -567,11 +534,11 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
float prompt_Q;
float tmp_E, tmp_P, tmp_L;
float tmp_float;
prompt_I = d_Prompt.imag();
prompt_Q = d_Prompt.real();
tmp_E=std::abs<float>(d_Early);
tmp_P=std::abs<float>(d_Prompt);
tmp_L=std::abs<float>(d_Late);
prompt_I = (*d_Prompt).imag();
prompt_Q = (*d_Prompt).real();
tmp_E=std::abs<float>(*d_Early);
tmp_P=std::abs<float>(*d_Prompt);
tmp_L=std::abs<float>(*d_Late);
try
{
// EPR
@ -655,10 +622,4 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::set_channel_queue(concurrent_queue<int>
void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_acquisition_gnss_synchro=p_gnss_synchro;
// Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
//DLOG(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
//DLOG(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz;
//DLOG(INFO) << "Tracking Satellite set to " << d_satellite;
}

View File

@ -46,7 +46,7 @@
//#include <gnuradio/gr_sync_decimator.h>
#include "concurrent_queue.h"
#include "gps_sdr_signal_processing.h"
#include "gnss_satellite.h"
#include "tracking_FLL_PLL_filter.h"
#include "gnss_synchro.h"
@ -88,6 +88,9 @@ public:
void update_local_code();
void update_local_carrier();
void set_FLL_and_PLL_BW(float fll_bw_hz,float pll_bw_hz);
/*
* \brief Satellite signal synchronization parameters uses shared memory between acquisition and tracking
*/
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
@ -96,11 +99,6 @@ public:
*
* The user must override work to define the signal processing code
*/
//virtual int work (int noutput_items,
// gr_vector_const_void_star &input_items,
// gr_vector_void_star &output_items) = 0;
//int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
@ -158,10 +156,11 @@ private:
gr_complex* d_carr_sign;
gr_complex d_Early;
gr_complex d_Prompt;
gr_complex* d_Early;
gr_complex* d_Prompt;
gr_complex* d_Late;
gr_complex d_Prompt_prev;
gr_complex d_Late;
float d_early_late_spc_chips;

View File

@ -125,14 +125,28 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
// Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2];
// Get space for the resampled early / prompt / late local replicas
d_early_code = new gr_complex[d_vector_length*2];
d_prompt_code = new gr_complex[d_vector_length*2];
d_late_code = new gr_complex[d_vector_length*2];
// space for carrier wipeoff and signal baseband vectors
d_carr_sign = new gr_complex[d_vector_length*2];
/* If an array is partitioned for more than one thread to operate on,
* having the sub-array boundaries unaligned to cache lines could lead
* to performance degradation. Here we allocate memory
* (gr_comlex array of size 2*d_vector_length) aligned to cache of 16 bytes
*/
// todo: do something if posix_memalign fails
// Get space for the resampled early / prompt / late local replicas
if (posix_memalign((void**)&d_early_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
if (posix_memalign((void**)&d_late_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
if (posix_memalign((void**)&d_prompt_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
// space for carrier wipeoff and signal baseband vectors
if (posix_memalign((void**)&d_carr_sign, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
// correlator outputs (scalar)
if (posix_memalign((void**)&d_Early, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Prompt, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){};
//--- Perform initializations ------------------------------
// define initial code frequency basis of NCO
d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ;
@ -300,11 +314,16 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_carrier()
Gps_L1_Ca_Dll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Pll_Tracking_cc()
{
d_dump_file.close();
free(d_prompt_code);
free(d_late_code);
free(d_early_code);
free(d_carr_sign);
free(d_Early);
free(d_Prompt);
free(d_Late);
delete[] d_ca_code;
delete[] d_early_code;
delete[] d_prompt_code;
delete[] d_late_code;
delete[] d_carr_sign;
delete[] d_Prompt_buffer;
}
@ -321,22 +340,11 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// if ((unsigned int)ninput_items[0]<(d_vector_length*2))
// {
// std::cout<<"End of signal detected\r\n";
// const int samples_available = ninput_items[0];
// consume_each(samples_available);
// return 0;
// }
// process vars
float carr_error;
float carr_nco;
float code_error;
float code_nco;
d_Early = gr_complex(0,0);
d_Prompt = gr_complex(0,0);
d_Late = gr_complex(0,0);
if (d_enable_tracking == true)
{
@ -395,31 +403,26 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
update_local_code();
update_local_carrier();
gr_complex bb_signal_sample(0,0);
// perform Early, Prompt and Late correlation
/*!
* \todo Use SIMD-enabled correlators
*/
for(int i=0; i<d_current_prn_length_samples; i++)
{
//Perform the carrier wipe-off
bb_signal_sample = in[i] * d_carr_sign[i];
// Now get early, late, and prompt values for each
d_Early += bb_signal_sample * d_early_code[i];
d_Prompt += bb_signal_sample * d_prompt_code[i];
d_Late += bb_signal_sample * d_late_code[i];
}
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);
// Compute PLL error and update carrier NCO -
carr_error = pll_cloop_two_quadrant_atan(d_Prompt) / (float)TWO_PI;
carr_error = pll_cloop_two_quadrant_atan(*d_Prompt) / (float)TWO_PI;
// Implement carrier loop filter and generate NCO command
carr_nco = d_carrier_loop_filter.get_carrier_nco(carr_error);
// Modify carrier freq based on NCO command
d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_nco;
// Compute DLL error and update code NCO
code_error = dll_nc_e_minus_l_normalized(d_Early, d_Late);
code_error = dll_nc_e_minus_l_normalized(*d_Early, *d_Late);
// Implement code loop filter and generate NCO command
code_nco = d_code_loop_filter.get_code_nco(code_error);
// Modify code freq based on NCO command
@ -463,7 +466,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
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_Prompt;
d_cn0_estimation_counter++;
}
else
@ -494,20 +497,9 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
}
// ########### Output the tracking data to navigation and PVT ##########
// // Output channel 0: Prompt correlator output Q
// *out[0] = (double)d_Prompt.real();
// // Output channel 1: Prompt correlator output I
// *out[1] = (double)d_Prompt.imag();
// // Output channel 2: PRN absolute delay [s]
// *out[2] = d_sample_counter_seconds;
// // Output channel 3: d_acc_carrier_phase_rad [rad]
// *out[3] = (double)d_acc_carrier_phase_rad;
// // Output channel 4: PRN code phase [s]
// *out[4] = (double)d_code_phase_samples * (1/(float)d_fs_in);
current_synchro_data.Prompt_I=(double)d_Prompt.real();
current_synchro_data.Prompt_Q=(double)d_Prompt.imag();
current_synchro_data.Prompt_I=(double)(*d_Prompt).real();
current_synchro_data.Prompt_Q=(double)(*d_Prompt).imag();
current_synchro_data.Tracking_timestamp_secs=d_sample_counter_seconds;
current_synchro_data.Carrier_phase_rads=(double)d_acc_carrier_phase_rad;
current_synchro_data.Code_phase_secs=(double)d_code_phase_samples * (1/(float)d_fs_in);
@ -542,25 +534,14 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
}
else
{
*d_Early = gr_complex(0,0);
*d_Prompt = gr_complex(0,0);
*d_Late = gr_complex(0,0);
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //block output streams pointer
//std::cout<<output_items.size()<<std::endl;
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data;
//void* current_synchro_data_b;
//char* p = (char*)malloc(sizeof(Gnss_Synchro));
//current_synchro_data_b= (void*) new (p) Gnss_Synchro();
//*((Gnss_Synchro*)current_synchro_data_b)=current_synchro_data;
//free(p);
//current_synchro_data.Acq_delay_samples=d_acquisition_gnss_synchro->Acq_delay_samples;
//current_synchro_data.Acq_doppler_hz=d_acquisition_gnss_synchro->Acq_doppler_hz;
//current_synchro_data.Acq_samplestamp_samples=d_acquisition_gnss_synchro->Acq_samplestamp_samples;
//current_synchro_data.Signal=d_acquisition_gnss_synchro->Signal; //copy the object signal
//std::cout<<noutput_items<<std::endl;
//std::cout<<"size of sizeof(Gnss_Synchro)="<<sizeof(Gnss_Synchro)<<std::endl;
*out[0]=current_synchro_data;
}
if(d_dump)
@ -570,11 +551,11 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
float prompt_Q;
float tmp_E, tmp_P, tmp_L;
float tmp_float;
prompt_I = d_Prompt.imag();
prompt_Q = d_Prompt.real();
tmp_E = std::abs<float>(d_Early);
tmp_P = std::abs<float>(d_Prompt);
tmp_L = std::abs<float>(d_Late);
prompt_I = (*d_Prompt).imag();
prompt_Q = (*d_Prompt).real();
tmp_E = std::abs<float>(*d_Early);
tmp_P = std::abs<float>(*d_Prompt);
tmp_L = std::abs<float>(*d_Late);
try
{
// EPR

View File

@ -45,11 +45,12 @@
//#include <gnuradio/gr_sync_decimator.h>
#include "concurrent_queue.h"
#include "gps_sdr_signal_processing.h"
#include "gnss_satellite.h"
#include "gnss_synchro.h"
#include "tracking_2nd_DLL_filter.h"
#include "tracking_2nd_PLL_filter.h"
#include "correlator.h"
class Gps_L1_Ca_Dll_Pll_Tracking_cc;
@ -88,11 +89,6 @@ public:
*
* The user must override work to define the signal processing code
*/
//virtual int work (int noutput_items,
// gr_vector_const_void_star &input_items,
// gr_vector_void_star &output_items) = 0;
//int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
@ -148,9 +144,9 @@ private:
gr_complex* d_prompt_code;
gr_complex* d_carr_sign;
gr_complex d_Early;
gr_complex d_Prompt;
gr_complex d_Late;
gr_complex *d_Early;
gr_complex *d_Prompt;
gr_complex *d_Late;
// remaining code phase and carrier phase between tracking loops
float d_rem_code_phase_samples;
@ -164,6 +160,8 @@ private:
// acquisition
float d_acq_code_phase_samples;
float d_acq_carrier_doppler_hz;
// correlator
Correlator d_correlator;
// tracking vars
float d_code_freq_hz;

View File

@ -133,6 +133,7 @@ void Correlator::cpu_arch_test_volk_32fc_x2_dot_prod_32fc_a()
// TODO: Make a test to find the best architecture
this->volk_32fc_x2_dot_prod_32fc_a_best_arch = arch_list.at(arch_list.size() - 1);
}
std::cout<<"Selected architecture for volk_32fc_x2_dot_prod_32fc_a is "<<this->volk_32fc_x2_dot_prod_32fc_a_best_arch<<std::endl;
}
@ -164,13 +165,14 @@ void Correlator::cpu_arch_test_volk_32fc_x2_multiply_32fc_a()
for (unsigned int i=0; i < arch_list.size(); ++i)
{
std::cout << "Arch " << i << ":" << arch_list.at(i) << std::endl;
if (arch_list.at(i).compare("sse3") == 1)
if (arch_list.at(i).find("sse")!=std::string::npos)
{
// TODO: Make a test to find the best architecture
this->volk_32fc_x2_multiply_32fc_a_best_arch = "sse3";
this->volk_32fc_x2_multiply_32fc_a_best_arch = arch_list.at(i);
}
}
}
std::cout<<"Selected architecture for volk_32fc_x2_multiply_32fc_a_best_arch is "<<this->volk_32fc_x2_multiply_32fc_a_best_arch<<std::endl;
}