[hamradio-commits] [gnss-sdr] 137/236: code cleaning
Carles Fernandez
carles_fernandez-guest at moszumanska.debian.org
Tue Apr 26 16:02:44 UTC 2016
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 70a2c5837cd8c6bbb888a94c944db3ac631cd479
Author: Carles Fernandez <carles.fernandez at gmail.com>
Date: Wed Mar 30 21:09:38 2016 +0200
code cleaning
---
.../galileo_e1b_telemetry_decoder_cc.cc | 4 +-
.../galileo_e5a_telemetry_decoder_cc.cc | 4 +-
.../gps_l1_ca_telemetry_decoder_cc.cc | 146 +++---
.../gps_l2_m_telemetry_decoder_cc.cc | 4 +-
.../sbas_l1_telemetry_decoder_cc.cc | 4 +-
.../gps_l1_ca_dll_fll_pll_tracking_cc.cc | 49 +-
.../gps_l1_ca_dll_pll_c_aid_tracking_cc.cc | 492 +++++++++++----------
.../gps_l1_ca_dll_pll_c_aid_tracking_sc.cc | 22 +-
8 files changed, 367 insertions(+), 358 deletions(-)
diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc
index b286aba..a6708eb 100644
--- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc
+++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc
@@ -120,8 +120,8 @@ galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc(
gr::block("galileo_e1b_telemetry_decoder_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
- // Telemetry Bit transition synchronization port out
- this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
+ // Telemetry Bit transition synchronization port out
+ this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
// initialize internal vars
d_queue = queue;
d_dump = dump;
diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc
index a00ab0f..1765cb1 100644
--- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc
+++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc
@@ -197,8 +197,8 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc(
gr::block("galileo_e5a_telemetry_decoder_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
- // Telemetry Bit transition synchronization port out
- this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
+ // Telemetry Bit transition synchronization port out
+ this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
// initialize internal vars
d_queue = queue;
d_dump = dump;
diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
index 2fb6e14..be09af2 100644
--- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
+++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
@@ -51,7 +51,7 @@ gps_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, boost::shared_ptr<
void gps_l1_ca_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{
- ninput_items_required[0] = GPS_CA_PREAMBLE_LENGTH_SYMBOLS; //set the required sample history
+ ninput_items_required[0] = GPS_CA_PREAMBLE_LENGTH_SYMBOLS; //set the required sample history
}
gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
@@ -61,8 +61,8 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
gr::block("gps_navigation_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
- // Telemetry Bit transition synchronization port out
- this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
+ // Telemetry Bit transition synchronization port out
+ this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
// initialize internal vars
d_queue = queue;
d_dump = dump;
@@ -156,82 +156,84 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
//******* preamble correlation ********
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].Prompt_I < 0) // symbols clipping
- {
- corr_value -= d_preambles_symbols[i]*in[0][i].correlation_length_ms;
- }
- else
- {
- corr_value += d_preambles_symbols[i]*in[0][i].correlation_length_ms;
- }
- }
- if (corr_value>=GPS_CA_PREAMBLE_LENGTH_SYMBOLS) break;
+ if (in[0][i].Flag_valid_symbol_output == true)
+ {
+ if (in[0][i].Prompt_I < 0) // symbols clipping
+ {
+ corr_value -= d_preambles_symbols[i] * in[0][i].correlation_length_ms;
+ }
+ else
+ {
+ corr_value += d_preambles_symbols[i] * in[0][i].correlation_length_ms;
+ }
+ }
+ if (corr_value >= GPS_CA_PREAMBLE_LENGTH_SYMBOLS) break;
}
d_flag_preamble = false;
//******* frame sync ******************
if (abs(corr_value) == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)
- {
- if (d_stat == 0)
- {
- d_GPS_FSM.Event_gps_word_preamble();
- 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;
- //sync the symbol to bits integrator
- d_symbol_accumulator = 0;
- d_symbol_accumulator_counter = 0;
- d_frame_bit_index = 0;
- d_stat = 1; // enter into frame pre-detection status
- }
- 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);
- 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;
- d_GPS_FSM.Event_gps_word_preamble();
- d_flag_preamble = true;
- 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)
- {
- //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);
- this->message_port_pub(pmt::mp("preamble_timestamp_s"),value);
+ {
+ if (d_stat == 0)
+ {
+ d_GPS_FSM.Event_gps_word_preamble();
+ 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);
+ //sync the symbol to bits integrator
+ d_symbol_accumulator = 0;
+ d_symbol_accumulator_counter = 0;
+ d_frame_bit_index = 0;
+ d_stat = 1; // enter into frame pre-detection status
+ }
+ 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);
+ 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);
+ d_GPS_FSM.Event_gps_word_preamble();
+ d_flag_preamble = true;
+ 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)
+ {
+ // 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);
+ this->message_port_pub(pmt::mp("preamble_timestamp_s"), value);
- d_flag_frame_sync = true;
- if (corr_value < 0)
- {
- 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();
- }
- else
- {
- flag_PLL_180_deg_phase_locked = false;
- }
- DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]";
- }
- }else{
- 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;
- d_stat = 0; //lost of frame sync
- d_flag_frame_sync = false;
- flag_TOW_set = false;
- }
- }
- }
- }
+ d_flag_frame_sync = true;
+ if (corr_value < 0)
+ {
+ 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();
+ }
+ else
+ {
+ flag_PLL_180_deg_phase_locked = false;
+ }
+ DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]";
+ }
+ }
+ else
+ {
+ if (preamble_diff_ms > GPS_SUBFRAME_MS+1)
+ {
+ DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff_ms= " << preamble_diff_ms;
+ d_stat = 0; // lost of frame sync
+ d_flag_frame_sync = false;
+ flag_TOW_set = false;
+ }
+ }
+ }
+ }
//******* SYMBOL TO BIT *******
- if (in[0][0].Flag_valid_symbol_output==true)
- {
- // 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_counter+=in[0][0].correlation_length_ms;
- }
+ if (in[0][0].Flag_valid_symbol_output == true)
+ {
+ // 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_counter += in[0][0].correlation_length_ms;
+ }
if (d_symbol_accumulator_counter == 20)
{
if (d_symbol_accumulator > 0)
@@ -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))
{
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_flag_parity = true;
}
@@ -397,7 +399,7 @@ void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel)
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "Telemetry decoder dump enabled on channel " << d_channel
- << " Log file: " << d_dump_filename.c_str();
+ << " Log file: " << d_dump_filename.c_str();
}
catch (std::ifstream::failure e)
{
diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.cc
index 2b4fa80..a49d3cf 100644
--- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.cc
+++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.cc
@@ -63,8 +63,8 @@ gps_l2_m_telemetry_decoder_cc::gps_l2_m_telemetry_decoder_cc(
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
- // Telemetry Bit transition synchronization port out
- this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
+ // Telemetry Bit transition synchronization port out
+ this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
// initialize internal vars
d_dump = dump;
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc
index 36af479..0261677 100644
--- a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc
+++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc
@@ -63,8 +63,8 @@ sbas_l1_telemetry_decoder_cc::sbas_l1_telemetry_decoder_cc(
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
- // Telemetry Bit transition synchronization port out
- this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
+ // Telemetry Bit transition synchronization port out
+ this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
// initialize internal vars
d_dump = dump;
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc
index 60f2614..7d82168 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc
@@ -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 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,
fs_in, vector_length, queue, dump, dump_filename, order, fll_bw_hz, pll_bw_hz,dll_bw_hz,
early_late_space_chips));
@@ -105,8 +104,8 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(
gr::block("Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
- // Telemetry bit synchronization message port input
- this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
+ // Telemetry bit synchronization message port input
+ this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
// initialize internal vars
d_queue = queue;
d_dump = dump;
@@ -289,9 +288,8 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_code()
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));
-
+ 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));
}
@@ -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,
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)
{
d_Prompt_prev = *d_Prompt;
- d_FLL_discriminator_hz=0.0;
+ d_FLL_discriminator_hz = 0.0;
d_FLL_wait = 0;
}
else
@@ -548,29 +545,29 @@ 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.Flag_valid_tracking = 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;
*out[0] = current_synchro_data;
}
else
{
- // ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled)
- /*!
- * \todo The stop timer has to be moved to the signal source!
- */
- // stream to collect cout calls to improve thread safety
- std::stringstream tmp_str_stream;
- if (floor(d_sample_counter / d_fs_in) != d_last_seg)
- {
- d_last_seg = floor(d_sample_counter / d_fs_in);
-
- if (d_channel == 0)
- {
- // debug: Second counter in channel 0
- tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush;
- std::cout << tmp_str_stream.rdbuf() << std::flush;
- }
- }
+ // ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled)
+ /*!
+ * \todo The stop timer has to be moved to the signal source!
+ */
+ // stream to collect cout calls to improve thread safety
+ std::stringstream tmp_str_stream;
+ if (floor(d_sample_counter / d_fs_in) != d_last_seg)
+ {
+ d_last_seg = floor(d_sample_counter / d_fs_in);
+
+ if (d_channel == 0)
+ {
+ // debug: Second counter in channel 0
+ tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush;
+ 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);
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
index 8751e05..f6b5392 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
@@ -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,
gr_vector_int &ninput_items_required)
{
@@ -87,16 +86,17 @@ 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)
{
- //pmt::print(msg);
- DLOG(INFO) << "Extended correlation enabled for Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)<< std::endl;
- if (d_enable_extended_integration==false) //avoid re-setting preamble indicator
- {
- d_preamble_timestamp_s=pmt::to_double(msg);
- d_enable_extended_integration=true;
- d_preamble_synchronized=false;
- }
+ //pmt::print(msg);
+ DLOG(INFO) << "Extended correlation enabled for Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)<< std::endl;
+ if (d_enable_extended_integration==false) //avoid re-setting preamble indicator
+ {
+ d_preamble_timestamp_s=pmt::to_double(msg);
+ d_enable_extended_integration = true;
+ d_preamble_synchronized = false;
+ }
}
@@ -116,11 +116,11 @@ gps_l1_ca_dll_pll_c_aid_tracking_cc::gps_l1_ca_dll_pll_c_aid_tracking_cc(
gr::block("gps_l1_ca_dll_pll_c_aid_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
- // Telemetry bit synchronization message port input
- this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
+ // Telemetry bit synchronization message port input
+ this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
- this->set_msg_handler(pmt::mp("preamble_timestamp_s"),
- boost::bind(&gps_l1_ca_dll_pll_c_aid_tracking_cc::msg_handler_preamble_index, this, _1));
+ this->set_msg_handler(pmt::mp("preamble_timestamp_s"),
+ boost::bind(&gps_l1_ca_dll_pll_c_aid_tracking_cc::msg_handler_preamble_index, this, _1));
// initialize internal vars
@@ -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);
// Initialize tracking ==========================================
- d_pll_bw_hz=pll_bw_hz;
- d_dll_bw_hz=dll_bw_hz;
- d_pll_bw_narrow_hz=pll_bw_narrow_hz;
- d_dll_bw_narrow_hz=dll_bw_narrow_hz;
+ d_pll_bw_hz = pll_bw_hz;
+ d_dll_bw_hz = dll_bw_hz;
+ d_pll_bw_narrow_hz = pll_bw_narrow_hz;
+ d_dll_bw_narrow_hz = dll_bw_narrow_hz;
d_extend_correlation_ms = extend_correlation_ms;
d_code_loop_filter.set_DLL_BW(d_dll_bw_hz);
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_code_phase_step_chips = 0.0;
d_carrier_phase_step_rad = 0.0;
- d_enable_extended_integration=false;
- d_preamble_synchronized=false;
+ d_enable_extended_integration = false;
+ d_preamble_synchronized = false;
//set_min_output_buffer((long int)300);
}
@@ -289,15 +289,14 @@ 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;
LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel;
-
// enable tracking
d_pull_in = true;
d_enable_tracking = true;
d_enable_extended_integration=false;
d_preamble_synchronized=false;
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;
+ << " Code Phase correction [samples]=" << delay_correction_samples
+ << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples;
}
@@ -346,8 +345,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec
d_pull_in = false;
// Fill the acquisition data
current_synchro_data = *d_acquisition_gnss_synchro;
- current_synchro_data.correlation_length_ms=1;
- current_synchro_data.Flag_valid_symbol_output = false;
+ current_synchro_data.correlation_length_ms = 1;
+ current_synchro_data.Flag_valid_symbol_output = false;
*out[0] = current_synchro_data;
consume_each(samples_offset); //shift input to perform alignment with local replica
return 1;
@@ -367,222 +366,230 @@ 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_L_history.push_back(d_correlator_outs[2]); // save late output
- if (static_cast<int>(d_P_history.size())>d_extend_correlation_ms)
- {
- d_E_history.pop_front();
- d_P_history.pop_front();
- d_L_history.pop_front();
- }
+ if (static_cast<int>(d_P_history.size()) > d_extend_correlation_ms)
+ {
+ d_E_history.pop_front();
+ d_P_history.pop_front();
+ d_L_history.pop_front();
+ }
bool enable_dll_pll;
- 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));
- if (symbol_diff>0 and symbol_diff % d_extend_correlation_ms == 0)
- {
- // compute coherent integration and enable tracking loop
- // perform coherent integration using correlator output history
- //std::cout<<"##### RESET COHERENT INTEGRATION ####"<<std::endl;
- d_correlator_outs[0]=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);
- for (int n=0;n<d_extend_correlation_ms;n++)
- {
- d_correlator_outs[0]+=d_E_history.at(n);
- d_correlator_outs[1]+=d_P_history.at(n);
- d_correlator_outs[2]+=d_L_history.at(n);
- }
-
- if (d_preamble_synchronized==false)
- {
- 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_preamble_synchronized=true;
- 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;
-
- }
- // UPDATE INTEGRATION TIME
- CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_extend_correlation_ms)*GPS_L1_CA_CODE_PERIOD;
- enable_dll_pll=true;
-
- }else{
- if(d_preamble_synchronized==true)
- {
- // continue extended coherent correlation
- //remnant carrier phase [rads]
- d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), GPS_TWO_PI);
-
- // Compute the next buffer length based on the period of the PRN sequence and the code phase error estimation
- double T_chip_seconds = 1 / d_code_freq_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);
- int K_prn_samples = round(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_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_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]
- d_code_phase_step_chips = 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));
-
- // UPDATE ACCUMULATED CARRIER PHASE
- 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;
-
- // disable tracking loop and inform telemetry decoder
- enable_dll_pll=false;
- }else{
- // perform basic (1ms) correlation
- // UPDATE INTEGRATION TIME
- CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
- enable_dll_pll=true;
- }
- }
- }else{
- // UPDATE INTEGRATION TIME
- CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
- enable_dll_pll=true;
- }
-
-
- if (enable_dll_pll==true)
- {
- // ################## PLL ##########################################################
- // Update PLL discriminator [rads/Ti -> Secs/Ti]
- d_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;
- // Input [s/Ti] -> output [Hz]
- d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, d_carr_phase_error_secs_Ti, CURRENT_INTEGRATION_TIME_S);
- // PLL to DLL assistance [Secs/Ti]
- d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / GPS_L1_FREQ_HZ;
- // code Doppler frequency update
- d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
-
- // ################## DLL ##########################################################
- // DLL discriminator
- 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
- 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;
- code_error_filt_secs_Ti = d_code_error_filt_chips_Ti/d_code_freq_chips; // [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.
- dll_code_error_secs_Ti = - code_error_filt_secs_Ti;// + d_pll_to_dll_assist_secs_Ti;
-
- // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
-
- // keep alignment parameters for the next input buffer
- double T_chip_seconds;
- double T_prn_seconds;
- double T_prn_samples;
- double K_prn_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 / 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_prn_samples = round(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;
- 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_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;
-
- // UPDATE ACCUMULATED CARRIER PHASE
- CORRECTED_INTEGRATION_TIME_S=(static_cast<double>(d_correlation_length_samples)/static_cast<double>(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 * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI);
- // UPDATE CARRIER PHASE ACCUULATOR
- //carrier phase accumulator prior to update the PLL estimators (accumulated carrier in this loop depends on the old estimations!)
- d_acc_carrier_phase_cycles -= d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S;
-
- //################### 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<double>(d_fs_in);
-
- //################### DLL COMMANDS #################################################
- //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);
- //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_correlator_outs[1]; //prompt
- d_cn0_estimation_counter++;
- }
- else
- {
- d_cn0_estimation_counter = 0;
- // Code lock indicator
- d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS);
- // Carrier lock indicator
- d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
- // Loss of lock detection
- if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0)
- {
- d_carrier_lock_fail_counter++;
- }
- else
- {
- if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
- }
- if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
- {
- std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
- LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
- std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
- if (d_queue != gr::msg_queue::sptr())
- {
- d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
- }
- d_carrier_lock_fail_counter = 0;
- d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
- }
- }
- // ########### Output the tracking data to navigation and PVT ##########
- 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!)
- 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;
- current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles;
- current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
- current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
- current_synchro_data.Flag_valid_pseudorange = false;
- current_synchro_data.Flag_valid_symbol_output = true;
- if (d_preamble_synchronized==true)
- {
- current_synchro_data.correlation_length_ms=d_extend_correlation_ms;
- }else{
- current_synchro_data.correlation_length_ms=1;
- }
- *out[0] = current_synchro_data;
- }else{
- 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!)
- current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + 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;
- current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles;
- current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;// todo: project the carrier doppler
- current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
- current_synchro_data.Flag_valid_pseudorange = false;
- current_synchro_data.Flag_valid_symbol_output = false;
- current_synchro_data.correlation_length_ms=1;
- *out[0] = current_synchro_data;
- }
+ 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));
+ if (symbol_diff>0 and symbol_diff % d_extend_correlation_ms == 0)
+ {
+ // compute coherent integration and enable tracking loop
+ // perform coherent integration using correlator output history
+ //std::cout<<"##### RESET COHERENT INTEGRATION ####"<<std::endl;
+ d_correlator_outs[0] = 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);
+ for (int n = 0; n < d_extend_correlation_ms; n++)
+ {
+ d_correlator_outs[0] += d_E_history.at(n);
+ d_correlator_outs[1] += d_P_history.at(n);
+ d_correlator_outs[2] += d_L_history.at(n);
+ }
+
+ if (d_preamble_synchronized == false)
+ {
+ 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_preamble_synchronized = true;
+ 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;
+ }
+ // UPDATE INTEGRATION TIME
+ CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_extend_correlation_ms) * GPS_L1_CA_CODE_PERIOD;
+ enable_dll_pll = true;
+
+ }
+ else
+ {
+ if(d_preamble_synchronized == true)
+ {
+ // continue extended coherent correlation
+ //remnant carrier phase [rads]
+ d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), GPS_TWO_PI);
+
+ // Compute the next buffer length based on the period of the PRN sequence and the code phase error estimation
+ double T_chip_seconds = 1 / d_code_freq_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);
+ int K_prn_samples = round(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_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_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]
+ d_code_phase_step_chips = 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));
+
+ // UPDATE ACCUMULATED CARRIER PHASE
+ 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;
+
+ // disable tracking loop and inform telemetry decoder
+ enable_dll_pll = false;
+ }
+ else
+ {
+ // perform basic (1ms) correlation
+ // UPDATE INTEGRATION TIME
+ CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
+ enable_dll_pll = true;
+ }
+ }
+ }
+ else
+ {
+ // UPDATE INTEGRATION TIME
+ CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
+ enable_dll_pll = true;
+ }
+
+ if (enable_dll_pll == true)
+ {
+ // ################## PLL ##########################################################
+ // Update PLL discriminator [rads/Ti -> Secs/Ti]
+ d_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;
+ // Input [s/Ti] -> output [Hz]
+ d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, d_carr_phase_error_secs_Ti, CURRENT_INTEGRATION_TIME_S);
+ // PLL to DLL assistance [Secs/Ti]
+ d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / GPS_L1_FREQ_HZ;
+ // code Doppler frequency update
+ d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
+
+ // ################## DLL ##########################################################
+ // DLL discriminator
+ 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
+ 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;
+ code_error_filt_secs_Ti = d_code_error_filt_chips_Ti / d_code_freq_chips; // [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.
+ dll_code_error_secs_Ti = - code_error_filt_secs_Ti; // + d_pll_to_dll_assist_secs_Ti;
+
+ // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
+
+ // keep alignment parameters for the next input buffer
+ double T_chip_seconds;
+ double T_prn_seconds;
+ double T_prn_samples;
+ double K_prn_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 / 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_prn_samples = round(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;
+ 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_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;
+
+ // UPDATE ACCUMULATED CARRIER PHASE
+ CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(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 * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI);
+ // UPDATE CARRIER PHASE ACCUULATOR
+ //carrier phase accumulator prior to update the PLL estimators (accumulated carrier in this loop depends on the old estimations!)
+ d_acc_carrier_phase_cycles -= d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S;
+
+ //################### 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<double>(d_fs_in);
+
+ //################### DLL COMMANDS #################################################
+ //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);
+ //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_correlator_outs[1]; //prompt
+ d_cn0_estimation_counter++;
+ }
+ else
+ {
+ d_cn0_estimation_counter = 0;
+ // Code lock indicator
+ d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS);
+ // Carrier lock indicator
+ d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
+ // Loss of lock detection
+ if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0)
+ {
+ d_carrier_lock_fail_counter++;
+ }
+ else
+ {
+ if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
+ }
+ if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
+ {
+ std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
+ LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
+ std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
+ if (d_queue != gr::msg_queue::sptr())
+ {
+ d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
+ }
+ d_carrier_lock_fail_counter = 0;
+ d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
+ }
+ }
+ // ########### Output the tracking data to navigation and PVT ##########
+ 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!)
+ 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;
+ current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles;
+ current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
+ current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
+ current_synchro_data.Flag_valid_pseudorange = false;
+ current_synchro_data.Flag_valid_symbol_output = true;
+ if (d_preamble_synchronized == true)
+ {
+ current_synchro_data.correlation_length_ms = d_extend_correlation_ms;
+ }
+ else
+ {
+ current_synchro_data.correlation_length_ms = 1;
+ }
+ *out[0] = current_synchro_data;
+ }
+ else
+ {
+ 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!)
+ current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + 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;
+ current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles;
+ current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;// todo: project the carrier doppler
+ current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
+ current_synchro_data.Flag_valid_pseudorange = false;
+ current_synchro_data.Flag_valid_symbol_output = false;
+ current_synchro_data.correlation_length_ms = 1;
+ *out[0] = current_synchro_data;
+ }
// ########## DEBUG OUTPUT
/*!
@@ -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);
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)
- << ", 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!
}
}
@@ -606,7 +613,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);
DLOG(INFO) << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
- << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]";
+ << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]";
}
}
}
@@ -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.Flag_valid_pseudorange = false;
- current_synchro_data.correlation_length_ms=1;
+ current_synchro_data.correlation_length_ms = 1;
*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
}
+
void gps_l1_ca_dll_pll_c_aid_tracking_cc::set_channel(unsigned int 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)
{
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)
{
d_acquisition_gnss_synchro = p_gnss_synchro;
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc
index 8e21de7..ae4c050 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc
@@ -102,8 +102,8 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
gr::block("gps_l1_ca_dll_pll_c_aid_tracking_sc", gr::io_signature::make(1, 1, sizeof(lv_16sc_t)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
- // Telemetry bit synchronization message port input
- this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
+ // Telemetry bit synchronization message port input
+ this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
// initialize internal vars
d_queue = queue;
d_dump = dump;
@@ -116,8 +116,8 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
// Initialize tracking ==========================================
d_pll_bw_hz=pll_bw_hz;
d_dll_bw_hz=dll_bw_hz;
- d_pll_bw_narrow_hz=pll_bw_narrow_hz;
- d_dll_bw_narrow_hz=dll_bw_narrow_hz;
+ d_pll_bw_narrow_hz = pll_bw_narrow_hz;
+ d_dll_bw_narrow_hz = dll_bw_narrow_hz;
d_code_loop_filter.set_DLL_BW(dll_bw_hz);
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;
LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel;
-
// enable tracking
d_pull_in = 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 ##############################
// 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);
- //std::cout<<std::endl;
// UPDATE INTEGRATION TIME
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);
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
// 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]
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
@@ -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.Flag_valid_pseudorange = false;
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;
// ########## 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
}
+
void gps_l1_ca_dll_pll_c_aid_tracking_sc::set_channel(unsigned int 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)
{
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)
{
d_acquisition_gnss_synchro = p_gnss_synchro;
--
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