mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-30 10:54:50 +00:00
Code Issues Releases Wiki Activity

code cleaning

Browse Source
This commit is contained in:
Carles Fernandez 2016-03-30 21:09:38 +02:00
parent b2034896e1
commit 70a2c5837c
8 changed files with 351 additions and 342 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@ -156,18 +156,18 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
//******* preamble correlation ******** //******* preamble correlation ********
for (unsigned int i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++) for (unsigned int i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++)
{ {
if (in[0][i].Flag_valid_symbol_output==true) if (in[0][i].Flag_valid_symbol_output == true)
{ {
if (in[0][i].Prompt_I < 0) // symbols clipping if (in[0][i].Prompt_I < 0) // symbols clipping
{ {
corr_value -= d_preambles_symbols[i]*in[0][i].correlation_length_ms; corr_value -= d_preambles_symbols[i] * in[0][i].correlation_length_ms;
} }
else else
{ {
corr_value += d_preambles_symbols[i]*in[0][i].correlation_length_ms; corr_value += d_preambles_symbols[i] * in[0][i].correlation_length_ms;
} }
} }
if (corr_value>=GPS_CA_PREAMBLE_LENGTH_SYMBOLS) break; if (corr_value >= GPS_CA_PREAMBLE_LENGTH_SYMBOLS) break;
} }
d_flag_preamble = false; d_flag_preamble = false;
@ -177,8 +177,8 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
if (d_stat == 0) if (d_stat == 0)
{ {
d_GPS_FSM.Event_gps_word_preamble(); d_GPS_FSM.Event_gps_word_preamble();
d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs;//record the preamble sample stamp d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs; // record the preamble sample stamp
DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "in[0][0].Tracking_timestamp_secs="<<round(in[0][0].Tracking_timestamp_secs * 1000.0) <<std::endl; DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "in[0][0].Tracking_timestamp_secs=" << round(in[0][0].Tracking_timestamp_secs * 1000.0);
//sync the symbol to bits integrator //sync the symbol to bits integrator
d_symbol_accumulator = 0; d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0; d_symbol_accumulator_counter = 0;
@ -187,23 +187,23 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
} }
else if (d_stat == 1) //check 6 seconds of preamble separation else if (d_stat == 1) //check 6 seconds of preamble separation
{ {
preamble_diff_ms = round((in[0][0].Tracking_timestamp_secs - d_preamble_time_seconds)*1000.0); preamble_diff_ms = round((in[0][0].Tracking_timestamp_secs - d_preamble_time_seconds) * 1000.0);
if (abs(preamble_diff_ms - GPS_SUBFRAME_MS) < 1) if (abs(preamble_diff_ms - GPS_SUBFRAME_MS) < 1)
{ {
DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite << "in[0][0].Tracking_timestamp_secs="<<round(in[0][0].Tracking_timestamp_secs * 1000.0) <<std::endl; DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite << "in[0][0].Tracking_timestamp_secs=" << round(in[0][0].Tracking_timestamp_secs * 1000.0);
d_GPS_FSM.Event_gps_word_preamble(); d_GPS_FSM.Event_gps_word_preamble();
d_flag_preamble = true; d_flag_preamble = true;
d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs;//record the PRN start sample index associated to the preamble d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs; // record the PRN start sample index associated to the preamble
if (!d_flag_frame_sync) if (!d_flag_frame_sync)
{ {
//send asynchronous message to tracking to inform of frame sync and extend correlation time // send asynchronous message to tracking to inform of frame sync and extend correlation time
pmt::pmt_t value = pmt::from_double(d_preamble_time_seconds-0.001); pmt::pmt_t value = pmt::from_double(d_preamble_time_seconds - 0.001);
this->message_port_pub(pmt::mp("preamble_timestamp_s"),value); this->message_port_pub(pmt::mp("preamble_timestamp_s"), value);
d_flag_frame_sync = true; d_flag_frame_sync = true;
if (corr_value < 0) if (corr_value < 0)
{ {
flag_PLL_180_deg_phase_locked = true; //PLL is locked to opposite phase! flag_PLL_180_deg_phase_locked = true; // PLL is locked to opposite phase!
DLOG(INFO) << " PLL in opposite phase for Sat "<< this->d_satellite.get_PRN(); DLOG(INFO) << " PLL in opposite phase for Sat "<< this->d_satellite.get_PRN();
} }
else else
@ -212,11 +212,13 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
} }
DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]"; DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]";
} }
}else{ }
else
{
if (preamble_diff_ms > GPS_SUBFRAME_MS+1) if (preamble_diff_ms > GPS_SUBFRAME_MS+1)
{ {
DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff_ms= " << preamble_diff_ms<<std::endl; DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff_ms= " << preamble_diff_ms;
d_stat = 0; //lost of frame sync d_stat = 0; // lost of frame sync
d_flag_frame_sync = false; d_flag_frame_sync = false;
flag_TOW_set = false; flag_TOW_set = false;
} }
@ -226,11 +228,11 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
//******* SYMBOL TO BIT ******* //******* SYMBOL TO BIT *******
if (in[0][0].Flag_valid_symbol_output==true) if (in[0][0].Flag_valid_symbol_output == true)
{ {
// extended correlation to bit period is enabled in tracking! // extended correlation to bit period is enabled in tracking!
d_symbol_accumulator += in[0][0].Prompt_I; // accumulate the input value in d_symbol_accumulator d_symbol_accumulator += in[0][0].Prompt_I; // accumulate the input value in d_symbol_accumulator
d_symbol_accumulator_counter+=in[0][0].correlation_length_ms; d_symbol_accumulator_counter += in[0][0].correlation_length_ms;
} }
if (d_symbol_accumulator_counter == 20) if (d_symbol_accumulator_counter == 20)
{ {
@ -267,7 +269,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(d_GPS_frame_4bytes)) if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(d_GPS_frame_4bytes))
{ {
memcpy(&d_GPS_FSM.d_GPS_frame_4bytes, &d_GPS_frame_4bytes, sizeof(char)*4); memcpy(&d_GPS_FSM.d_GPS_frame_4bytes, &d_GPS_frame_4bytes, sizeof(char)*4);
d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds*1000.0; d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds * 1000.0;
d_GPS_FSM.Event_gps_word_valid(); d_GPS_FSM.Event_gps_word_valid();
d_flag_parity = true; d_flag_parity = true;
} }

11
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc
View File

@ -72,7 +72,6 @@ gps_l1_ca_dll_fll_pll_tracking_cc_sptr gps_l1_ca_dll_fll_pll_make_tracking_cc(
float dll_bw_hz, float dll_bw_hz,
float early_late_space_chips) float early_late_space_chips)
{ {
return gps_l1_ca_dll_fll_pll_tracking_cc_sptr(new Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(if_freq, return gps_l1_ca_dll_fll_pll_tracking_cc_sptr(new Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(if_freq,
fs_in, vector_length, queue, dump, dump_filename, order, fll_bw_hz, pll_bw_hz,dll_bw_hz, fs_in, vector_length, queue, dump, dump_filename, order, fll_bw_hz, pll_bw_hz,dll_bw_hz,
early_late_space_chips)); early_late_space_chips));
@ -289,9 +288,8 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_code()
tcode_chips = tcode_chips + code_phase_step_chips; 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_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)); memcpy(d_late_code, &d_early_code[early_late_spc_samples*2], d_current_prn_length_samples * sizeof(gr_complex));
} }
@ -332,7 +330,6 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc()
int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items, int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{ {
@ -433,7 +430,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
if (d_FLL_wait == 1) if (d_FLL_wait == 1)
{ {
d_Prompt_prev = *d_Prompt; d_Prompt_prev = *d_Prompt;
d_FLL_discriminator_hz=0.0; d_FLL_discriminator_hz = 0.0;
d_FLL_wait = 0; d_FLL_wait = 0;
} }
else else
@ -548,7 +545,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_tracking = true; current_synchro_data.Flag_valid_tracking = true;
current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.Flag_valid_symbol_output = true;
current_synchro_data.correlation_length_ms=1; current_synchro_data.correlation_length_ms = 1;
current_synchro_data.Flag_valid_pseudorange = false; current_synchro_data.Flag_valid_pseudorange = false;
*out[0] = current_synchro_data; *out[0] = current_synchro_data;
} }

128
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
View File

@ -77,7 +77,6 @@ gps_l1_ca_dll_pll_c_aid_make_tracking_cc(
} }
void gps_l1_ca_dll_pll_c_aid_tracking_cc::forecast (int noutput_items, void gps_l1_ca_dll_pll_c_aid_tracking_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required) gr_vector_int &ninput_items_required)
{ {
@ -87,6 +86,7 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::forecast (int noutput_items,
} }
} }
void gps_l1_ca_dll_pll_c_aid_tracking_cc::msg_handler_preamble_index(pmt::pmt_t msg) void gps_l1_ca_dll_pll_c_aid_tracking_cc::msg_handler_preamble_index(pmt::pmt_t msg)
{ {
//pmt::print(msg); //pmt::print(msg);
@ -94,8 +94,8 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::msg_handler_preamble_index(pmt::pmt_t
if (d_enable_extended_integration==false) //avoid re-setting preamble indicator if (d_enable_extended_integration==false) //avoid re-setting preamble indicator
{ {
d_preamble_timestamp_s=pmt::to_double(msg); d_preamble_timestamp_s=pmt::to_double(msg);
d_enable_extended_integration=true; d_enable_extended_integration = true;
d_preamble_synchronized=false; d_preamble_synchronized = false;
} }
} }
@ -133,10 +133,10 @@ gps_l1_ca_dll_pll_c_aid_tracking_cc::gps_l1_ca_dll_pll_c_aid_tracking_cc(
d_correlation_length_samples = static_cast<int>(d_vector_length); d_correlation_length_samples = static_cast<int>(d_vector_length);
// Initialize tracking ========================================== // Initialize tracking ==========================================
d_pll_bw_hz=pll_bw_hz; d_pll_bw_hz = pll_bw_hz;
d_dll_bw_hz=dll_bw_hz; d_dll_bw_hz = dll_bw_hz;
d_pll_bw_narrow_hz=pll_bw_narrow_hz; d_pll_bw_narrow_hz = pll_bw_narrow_hz;
d_dll_bw_narrow_hz=dll_bw_narrow_hz; d_dll_bw_narrow_hz = dll_bw_narrow_hz;
d_extend_correlation_ms = extend_correlation_ms; d_extend_correlation_ms = extend_correlation_ms;
d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); d_code_loop_filter.set_DLL_BW(d_dll_bw_hz);
d_carrier_loop_filter.set_params(10.0, d_pll_bw_hz,2); d_carrier_loop_filter.set_params(10.0, d_pll_bw_hz,2);
@ -208,8 +208,8 @@ gps_l1_ca_dll_pll_c_aid_tracking_cc::gps_l1_ca_dll_pll_c_aid_tracking_cc(
d_rem_code_phase_chips = 0.0; d_rem_code_phase_chips = 0.0;
d_code_phase_step_chips = 0.0; d_code_phase_step_chips = 0.0;
d_carrier_phase_step_rad = 0.0; d_carrier_phase_step_rad = 0.0;
d_enable_extended_integration=false; d_enable_extended_integration = false;
d_preamble_synchronized=false; d_preamble_synchronized = false;
//set_min_output_buffer((long int)300); //set_min_output_buffer((long int)300);
} }
@ -289,7 +289,6 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl;
LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel;
// enable tracking // enable tracking
d_pull_in = true; d_pull_in = true;
d_enable_tracking = true; d_enable_tracking = true;
@ -346,7 +345,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec
d_pull_in = false; d_pull_in = false;
// Fill the acquisition data // Fill the acquisition data
current_synchro_data = *d_acquisition_gnss_synchro; current_synchro_data = *d_acquisition_gnss_synchro;
current_synchro_data.correlation_length_ms=1; current_synchro_data.correlation_length_ms = 1;
current_synchro_data.Flag_valid_symbol_output = false; current_synchro_data.Flag_valid_symbol_output = false;
*out[0] = current_synchro_data; *out[0] = current_synchro_data;
consume_each(samples_offset); //shift input to perform alignment with local replica consume_each(samples_offset); //shift input to perform alignment with local replica
@ -367,7 +366,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec
d_P_history.push_back(d_correlator_outs[1]); // save prompt output d_P_history.push_back(d_correlator_outs[1]); // save prompt output
d_L_history.push_back(d_correlator_outs[2]); // save late output d_L_history.push_back(d_correlator_outs[2]); // save late output
if (static_cast<int>(d_P_history.size())>d_extend_correlation_ms) if (static_cast<int>(d_P_history.size()) > d_extend_correlation_ms)
{ {
d_E_history.pop_front(); d_E_history.pop_front();
d_P_history.pop_front(); d_P_history.pop_front();
@ -375,39 +374,40 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec
} }
bool enable_dll_pll; bool enable_dll_pll;
if (d_enable_extended_integration==true) if (d_enable_extended_integration == true)
{ {
long int symbol_diff=round(1000.0*((static_cast<double>(d_sample_counter) + d_rem_code_phase_samples) / static_cast<double>(d_fs_in)-d_preamble_timestamp_s)); long int symbol_diff = round(1000.0*((static_cast<double>(d_sample_counter) + d_rem_code_phase_samples) / static_cast<double>(d_fs_in)-d_preamble_timestamp_s));
if (symbol_diff>0 and symbol_diff % d_extend_correlation_ms == 0) if (symbol_diff>0 and symbol_diff % d_extend_correlation_ms == 0)
{ {
// compute coherent integration and enable tracking loop // compute coherent integration and enable tracking loop
// perform coherent integration using correlator output history // perform coherent integration using correlator output history
//std::cout<<"##### RESET COHERENT INTEGRATION ####"<<std::endl; //std::cout<<"##### RESET COHERENT INTEGRATION ####"<<std::endl;
d_correlator_outs[0]=gr_complex(0.0,0.0); d_correlator_outs[0] = gr_complex(0.0,0.0);
d_correlator_outs[1]=gr_complex(0.0,0.0); d_correlator_outs[1] = gr_complex(0.0,0.0);
d_correlator_outs[2]=gr_complex(0.0,0.0); d_correlator_outs[2] = gr_complex(0.0,0.0);
for (int n=0;n<d_extend_correlation_ms;n++) for (int n = 0; n < d_extend_correlation_ms; n++)
{ {
d_correlator_outs[0]+=d_E_history.at(n); d_correlator_outs[0] += d_E_history.at(n);
d_correlator_outs[1]+=d_P_history.at(n); d_correlator_outs[1] += d_P_history.at(n);
d_correlator_outs[2]+=d_L_history.at(n); d_correlator_outs[2] += d_L_history.at(n);
} }
if (d_preamble_synchronized==false) if (d_preamble_synchronized == false)
{ {
d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz);
d_carrier_loop_filter.set_params(10.0, d_pll_bw_narrow_hz,2); d_carrier_loop_filter.set_params(10.0, d_pll_bw_narrow_hz,2);
d_preamble_synchronized=true; d_preamble_synchronized = true;
std::cout<<"Enabled extended correlator for CH "<< d_channel <<" : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) std::cout << "Enabled extended correlator for CH "<< d_channel <<" : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<<" dll_narrow_bw="<<d_dll_bw_narrow_hz<<" pll_narrow_bw="<<d_pll_bw_narrow_hz<<std::endl; <<" dll_narrow_bw=" << d_dll_bw_narrow_hz << " pll_narrow_bw=" << d_pll_bw_narrow_hz << std::endl;
} }
// UPDATE INTEGRATION TIME // UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_extend_correlation_ms)*GPS_L1_CA_CODE_PERIOD; CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_extend_correlation_ms) * GPS_L1_CA_CODE_PERIOD;
enable_dll_pll=true; enable_dll_pll = true;
}else{ }
if(d_preamble_synchronized==true) else
{
if(d_preamble_synchronized == true)
{ {
// continue extended coherent correlation // continue extended coherent correlation
//remnant carrier phase [rads] //remnant carrier phase [rads]
@ -418,38 +418,41 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec
double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in); double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
int K_prn_samples = round(T_prn_samples); int K_prn_samples = round(T_prn_samples);
double K_T_prn_error_samples=K_prn_samples-T_prn_samples; double K_T_prn_error_samples = K_prn_samples - T_prn_samples;
d_rem_code_phase_samples= d_rem_code_phase_samples - K_T_prn_error_samples; d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples;
d_rem_code_phase_integer_samples=round(d_rem_code_phase_samples); d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples);
d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; //round to a discrete samples d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; //round to a discrete samples
d_rem_code_phase_samples=d_rem_code_phase_samples-d_rem_code_phase_integer_samples; d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples;
//code phase step (Code resampler phase increment per sample) [chips/sample] //code phase step (Code resampler phase increment per sample) [chips/sample]
d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in); d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
//remnant code phase [chips] //remnant code phase [chips]
d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in)); d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in));
// UPDATE ACCUMULATED CARRIER PHASE // UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S=(static_cast<double>(d_correlation_length_samples)/static_cast<double>(d_fs_in)); CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
d_acc_carrier_phase_cycles -= d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S; d_acc_carrier_phase_cycles -= d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S;
// disable tracking loop and inform telemetry decoder // disable tracking loop and inform telemetry decoder
enable_dll_pll=false; enable_dll_pll = false;
}else{ }
else
{
// perform basic (1ms) correlation // perform basic (1ms) correlation
// UPDATE INTEGRATION TIME // UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in); CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
enable_dll_pll=true; enable_dll_pll = true;
} }
} }
}else{ }
else
{
// UPDATE INTEGRATION TIME // UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in); CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
enable_dll_pll=true; enable_dll_pll = true;
} }
if (enable_dll_pll == true)
if (enable_dll_pll==true)
{ {
// ################## PLL ########################################################## // ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti] // Update PLL discriminator [rads/Ti -> Secs/Ti]
@ -469,11 +472,11 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec
d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); //[chips/Ti] //early and late d_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 discriminator filter
d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); //input [chips/Ti] -> output [chips/second] d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); //input [chips/Ti] -> output [chips/second]
d_code_error_filt_chips_Ti = d_code_error_filt_chips_s*CURRENT_INTEGRATION_TIME_S; d_code_error_filt_chips_Ti = d_code_error_filt_chips_s * CURRENT_INTEGRATION_TIME_S;
code_error_filt_secs_Ti = d_code_error_filt_chips_Ti/d_code_freq_chips; // [s/Ti] code_error_filt_secs_Ti = d_code_error_filt_chips_Ti / d_code_freq_chips; // [s/Ti]
// DLL code error estimation [s/Ti] // DLL code error estimation [s/Ti]
// PLL to DLL assistance is disable due to the use of a fractional resampler that allows the correction of the code Doppler effect. // PLL to DLL assistance is disable due to the use of a fractional resampler that allows the correction of the code Doppler effect.
dll_code_error_secs_Ti = - code_error_filt_secs_Ti;// + d_pll_to_dll_assist_secs_Ti; dll_code_error_secs_Ti = - code_error_filt_secs_Ti; // + d_pll_to_dll_assist_secs_Ti;
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
@ -487,16 +490,16 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec
T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_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); T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
K_prn_samples = round(T_prn_samples); K_prn_samples = round(T_prn_samples);
double K_T_prn_error_samples=K_prn_samples-T_prn_samples; double K_T_prn_error_samples = K_prn_samples - T_prn_samples;
old_d_rem_code_phase_samples=d_rem_code_phase_samples; old_d_rem_code_phase_samples = d_rem_code_phase_samples;
d_rem_code_phase_samples= d_rem_code_phase_samples - K_T_prn_error_samples -dll_code_error_secs_Ti * static_cast<double>(d_fs_in); d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples - dll_code_error_secs_Ti * static_cast<double>(d_fs_in);
d_rem_code_phase_integer_samples=round(d_rem_code_phase_samples); d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples);
d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; //round to a discrete samples d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; //round to a discrete samples
d_rem_code_phase_samples=d_rem_code_phase_samples-d_rem_code_phase_integer_samples; d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples;
// UPDATE ACCUMULATED CARRIER PHASE // UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S=(static_cast<double>(d_correlation_length_samples)/static_cast<double>(d_fs_in)); CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
//remnant carrier phase [rad] //remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI); d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI);
// UPDATE CARRIER PHASE ACCUULATOR // UPDATE CARRIER PHASE ACCUULATOR
@ -561,14 +564,18 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_pseudorange = false; current_synchro_data.Flag_valid_pseudorange = false;
current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.Flag_valid_symbol_output = true;
if (d_preamble_synchronized==true) if (d_preamble_synchronized == true)
{ {
current_synchro_data.correlation_length_ms=d_extend_correlation_ms; current_synchro_data.correlation_length_ms = d_extend_correlation_ms;
}else{ }
current_synchro_data.correlation_length_ms=1; else
{
current_synchro_data.correlation_length_ms = 1;
} }
*out[0] = current_synchro_data; *out[0] = current_synchro_data;
}else{ }
else
{
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real()); 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()); 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!) // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!)
@ -580,7 +587,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_pseudorange = false; current_synchro_data.Flag_valid_pseudorange = false;
current_synchro_data.Flag_valid_symbol_output = false; current_synchro_data.Flag_valid_symbol_output = false;
current_synchro_data.correlation_length_ms=1; current_synchro_data.correlation_length_ms = 1;
*out[0] = current_synchro_data; *out[0] = current_synchro_data;
} }
@ -596,7 +603,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec
d_last_seg = floor(d_sample_counter / d_fs_in); d_last_seg = floor(d_sample_counter / d_fs_in);
std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl; std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl;
DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]";
//if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock! //if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
} }
} }
@ -636,7 +643,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec
current_synchro_data.System = {'G'}; current_synchro_data.System = {'G'};
current_synchro_data.Flag_valid_pseudorange = false; current_synchro_data.Flag_valid_pseudorange = false;
current_synchro_data.correlation_length_ms=1; current_synchro_data.correlation_length_ms = 1;
*out[0] = current_synchro_data; *out[0] = current_synchro_data;
} }
@ -705,6 +712,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
} }
void gps_l1_ca_dll_pll_c_aid_tracking_cc::set_channel(unsigned int channel) void gps_l1_ca_dll_pll_c_aid_tracking_cc::set_channel(unsigned int channel)
{ {
d_channel = channel; d_channel = channel;
@ -730,11 +738,13 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::set_channel(unsigned int channel)
} }
} }
void gps_l1_ca_dll_pll_c_aid_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue) void gps_l1_ca_dll_pll_c_aid_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{ {
d_channel_internal_queue = channel_internal_queue; d_channel_internal_queue = channel_internal_queue;
} }
void gps_l1_ca_dll_pll_c_aid_tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) void gps_l1_ca_dll_pll_c_aid_tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
d_acquisition_gnss_synchro = p_gnss_synchro; d_acquisition_gnss_synchro = p_gnss_synchro;

18
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc
View File

@ -116,8 +116,8 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
// Initialize tracking ========================================== // Initialize tracking ==========================================
d_pll_bw_hz=pll_bw_hz; d_pll_bw_hz=pll_bw_hz;
d_dll_bw_hz=dll_bw_hz; d_dll_bw_hz=dll_bw_hz;
d_pll_bw_narrow_hz=pll_bw_narrow_hz; d_pll_bw_narrow_hz = pll_bw_narrow_hz;
d_dll_bw_narrow_hz=dll_bw_narrow_hz; d_dll_bw_narrow_hz = dll_bw_narrow_hz;
d_code_loop_filter.set_DLL_BW(dll_bw_hz); d_code_loop_filter.set_DLL_BW(dll_bw_hz);
d_carrier_loop_filter.set_params(10.0, pll_bw_hz,2); d_carrier_loop_filter.set_params(10.0, pll_bw_hz,2);
@ -269,7 +269,6 @@ void gps_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking()
std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl;
LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel;
// enable tracking // enable tracking
d_pull_in = true; d_pull_in = true;
d_enable_tracking = true; d_enable_tracking = true;
@ -339,11 +338,9 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items, gr_vec
// ################# CARRIER WIPEOFF AND CORRELATORS ############################## // ################# CARRIER WIPEOFF AND CORRELATORS ##############################
// perform carrier wipe-off and compute Early, Prompt and Late correlation // perform carrier wipe-off and compute Early, Prompt and Late correlation
multicorrelator_cpu_16sc.set_input_output_vectors(d_correlator_outs_16sc, in);
multicorrelator_cpu_16sc.set_input_output_vectors(d_correlator_outs_16sc,in);
multicorrelator_cpu_16sc.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_correlation_length_samples); multicorrelator_cpu_16sc.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_correlation_length_samples);
//std::cout<<std::endl;
// UPDATE INTEGRATION TIME // UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in); CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
@ -383,11 +380,11 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items, gr_vec
K_blk_samples = T_prn_samples + d_rem_code_phase_samples - dll_code_error_secs_Ti * static_cast<double>(d_fs_in); K_blk_samples = T_prn_samples + d_rem_code_phase_samples - dll_code_error_secs_Ti * static_cast<double>(d_fs_in);
d_correlation_length_samples = round(K_blk_samples); //round to a discrete samples d_correlation_length_samples = round(K_blk_samples); //round to a discrete samples
old_d_rem_code_phase_samples=d_rem_code_phase_samples; old_d_rem_code_phase_samples = d_rem_code_phase_samples;
d_rem_code_phase_samples = K_blk_samples - static_cast<double>(d_correlation_length_samples); //rounding error < 1 sample d_rem_code_phase_samples = K_blk_samples - static_cast<double>(d_correlation_length_samples); //rounding error < 1 sample
// UPDATE REMNANT CARRIER PHASE // UPDATE REMNANT CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S=(static_cast<double>(d_correlation_length_samples)/static_cast<double>(d_fs_in)); CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
//remnant carrier phase [rad] //remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI); d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI);
// UPDATE CARRIER PHASE ACCUULATOR // UPDATE CARRIER PHASE ACCUULATOR
@ -453,7 +450,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items, gr_vec
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_pseudorange = false; current_synchro_data.Flag_valid_pseudorange = false;
current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.Flag_valid_symbol_output = true;
current_synchro_data.correlation_length_ms=1; current_synchro_data.correlation_length_ms = 1;
*out[0] = current_synchro_data; *out[0] = current_synchro_data;
// ########## DEBUG OUTPUT // ########## DEBUG OUTPUT
@ -577,6 +574,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items, gr_vec
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
} }
void gps_l1_ca_dll_pll_c_aid_tracking_sc::set_channel(unsigned int channel) void gps_l1_ca_dll_pll_c_aid_tracking_sc::set_channel(unsigned int channel)
{ {
d_channel = channel; d_channel = channel;
@ -602,11 +600,13 @@ void gps_l1_ca_dll_pll_c_aid_tracking_sc::set_channel(unsigned int channel)
} }
} }
void gps_l1_ca_dll_pll_c_aid_tracking_sc::set_channel_queue(concurrent_queue<int> *channel_internal_queue) void gps_l1_ca_dll_pll_c_aid_tracking_sc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{ {
d_channel_internal_queue = channel_internal_queue; d_channel_internal_queue = channel_internal_queue;
} }
void gps_l1_ca_dll_pll_c_aid_tracking_sc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) void gps_l1_ca_dll_pll_c_aid_tracking_sc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
d_acquisition_gnss_synchro = p_gnss_synchro; d_acquisition_gnss_synchro = p_gnss_synchro;