From a35b90b4cb589de080828c31b93a50e278bb1ee0 Mon Sep 17 00:00:00 2001 From: Vladisslav P Date: Tue, 12 Jul 2022 05:28:49 +0300 Subject: [PATCH 1/2] glonass_lx_ca_dll_pll_c_aid_tracking better checks before enabling preamble_timestamp_samples message port connection 1. Check for message port presence both in trk and nav blocks 2. Check for configuration property Tracking_xG.extend_correlation_ms Signed-off-by: Vladisslav P --- src/algorithms/channel/adapters/channel.cc | 32 ++++++++++++++++++++-- src/algorithms/channel/adapters/channel.h | 2 ++ 2 files changed, 32 insertions(+), 2 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index 28376dfd8..adb392e9f 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -104,6 +104,9 @@ Channel::Channel(const ConfigurationInterface* configuration, gnss_signal_ = Gnss_Signal(signal_str); channel_msg_rx_ = channel_msg_receiver_make_cc(channel_fsm_, repeat_); + + glonass_extend_correlation_ms_ = 0; + glonass_extend_correlation_ms_ = configuration->property("Tracking_1G.extend_correlation_ms", 0) + configuration->property("Tracking_2G.extend_correlation_ms", 0); } @@ -128,7 +131,7 @@ void Channel::connect(gr::top_block_sptr top_block) // Message ports top_block->msg_connect(nav_->get_left_block(), pmt::mp("telemetry_to_trk"), trk_->get_right_block(), pmt::mp("telemetry_to_trk")); - if ((trk_->get_right_block()->name() == "glonass_l1_ca_dll_pll_c_aid_tracking_cc") || (trk_->get_right_block()->name() == "glonass_l2_ca_dll_pll_c_aid_tracking_cc")) + if (glonass_dll_pll_c_aid_tracking_check()) { top_block->msg_connect(nav_->get_left_block(), pmt::mp("preamble_timestamp_samples"), trk_->get_right_block(), pmt::mp("preamble_timestamp_samples")); } @@ -162,7 +165,7 @@ void Channel::disconnect(gr::top_block_sptr top_block) nav_->disconnect(top_block); top_block->msg_disconnect(nav_->get_left_block(), pmt::mp("telemetry_to_trk"), trk_->get_right_block(), pmt::mp("telemetry_to_trk")); - if ((trk_->get_right_block()->name() == "glonass_l1_ca_dll_pll_c_aid_tracking_cc") || (trk_->get_right_block()->name() == "glonass_l2_ca_dll_pll_c_aid_tracking_cc")) + if (glonass_dll_pll_c_aid_tracking_check()) { top_block->msg_disconnect(nav_->get_left_block(), pmt::mp("preamble_timestamp_samples"), trk_->get_right_block(), pmt::mp("preamble_timestamp_samples")); } @@ -270,3 +273,28 @@ void Channel::start_acquisition() } DLOG(INFO) << "Channel start_acquisition()"; } + +bool Channel::glonass_dll_pll_c_aid_tracking_check() +{ + if (glonass_extend_correlation_ms_) + { + const pmt::pmt_t nav_ports_out = nav_->get_left_block()->message_ports_out(); + const pmt::pmt_t trk_ports_in = trk_->get_right_block()->message_ports_in(); + const pmt::pmt_t symbol = pmt::mp("preamble_timestamp_samples"); + for (unsigned k = 0; k < pmt::length(nav_ports_out); k++) + { + if (pmt::vector_ref(nav_ports_out, k) == symbol) + { + for (unsigned j = 0; j < pmt::length(trk_ports_in); j++) + { + if (pmt::vector_ref(trk_ports_in, j) == symbol) + { + return true; + } + } + return false; + } + } + } + return false; +} diff --git a/src/algorithms/channel/adapters/channel.h b/src/algorithms/channel/adapters/channel.h index d43d1df42..b9d3dbfc6 100644 --- a/src/algorithms/channel/adapters/channel.h +++ b/src/algorithms/channel/adapters/channel.h @@ -96,6 +96,7 @@ public: inline std::shared_ptr telemetry() const { return nav_; } private: + bool glonass_dll_pll_c_aid_tracking_check(); std::shared_ptr channel_fsm_; std::shared_ptr acq_; std::shared_ptr trk_; @@ -109,6 +110,7 @@ private: bool connected_; bool repeat_; bool flag_enable_fpga_; + int glonass_extend_correlation_ms_; }; From c991f8f601da64ad119d0e462d03bb600efcdb4c Mon Sep 17 00:00:00 2001 From: Vladisslav P Date: Tue, 12 Jul 2022 06:48:24 +0300 Subject: [PATCH 2/2] Glonass: implement extended correlation for cshort blocks Signed-off-by: Vladisslav P --- .../glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc | 17 +++++++++-------- .../glonass_l1_ca_dll_pll_c_aid_tracking_sc.h | 2 +- .../glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc | 17 +++++++++-------- .../glonass_l2_ca_dll_pll_c_aid_tracking_sc.h | 2 +- 4 files changed, 20 insertions(+), 18 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc index 6ca1c9030..27d62058a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc @@ -85,7 +85,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::msg_handler_preamble_index(const p DLOG(INFO) << "Extended correlation enabled for Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); if (d_enable_extended_integration == false) // avoid re-setting preamble indicator { - d_preamble_timestamp_s = pmt::to_double(msg); + d_preamble_timestamp_samples = pmt::to_double(msg); d_enable_extended_integration = true; d_preamble_synchronized = false; } @@ -134,7 +134,7 @@ glonass_l1_ca_dll_pll_c_aid_tracking_sc::glonass_l1_ca_dll_pll_c_aid_tracking_sc d_pll_to_dll_assist_secs_Ti(0.0), d_carr_phase_error_secs_Ti(0.0), d_code_error_chips_Ti(0.0), - d_preamble_timestamp_s(0.0), + d_preamble_timestamp_samples(0.0), d_extend_correlation_ms(extend_correlation_ms), d_code_error_filt_chips_s(0.0), d_code_error_filt_chips_Ti(0.0), @@ -154,8 +154,8 @@ glonass_l1_ca_dll_pll_c_aid_tracking_sc::glonass_l1_ca_dll_pll_c_aid_tracking_sc d_dump(dump) { // 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"), + this->message_port_register_in(pmt::mp("preamble_timestamp_samples")); + this->set_msg_handler(pmt::mp("preamble_timestamp_samples"), #if HAS_GENERIC_LAMBDA [this](auto &&PH1) { msg_handler_preamble_index(PH1); }); #else @@ -209,7 +209,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking() const double acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); // Doppler effect // Fd=(C/(C+Vr))*F - d_glonass_freq_ch = GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_FREQ_HZ * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + d_glonass_freq_ch = GLONASS_L1_CA_FREQ_HZ + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); const double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch; // new chip and prn sequence periods based on acq Doppler d_code_freq_chips = radial_velocity * GLONASS_L1_CA_CODE_RATE_CPS; @@ -273,8 +273,8 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking() // enable tracking d_pull_in = true; d_enable_tracking = true; - d_enable_extended_integration = true; - d_preamble_synchronized = 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 @@ -604,7 +604,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at bool enable_dll_pll; if (d_enable_extended_integration == true) { - int64_t symbol_diff = round(1000.0 * ((static_cast(d_sample_counter) + d_rem_code_phase_samples) / static_cast(d_fs_in) - d_preamble_timestamp_s)); + int64_t symbol_diff = round(1000.0 * ((static_cast(d_sample_counter) + d_rem_code_phase_samples - d_preamble_timestamp_samples) / static_cast(d_fs_in))); if (symbol_diff > 0 and symbol_diff % d_extend_correlation_ms == 0) { // compute coherent integration and enable tracking loop @@ -631,6 +631,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at } // UPDATE INTEGRATION TIME CURRENT_INTEGRATION_TIME_S = static_cast(d_extend_correlation_ms) * GLONASS_L1_CA_CODE_PERIOD_S; + d_code_loop_filter.set_pdi(static_cast(CURRENT_INTEGRATION_TIME_S)); enable_dll_pll = true; } else diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h index 8c482ec92..999b1f7a2 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h @@ -170,7 +170,7 @@ private: double d_pll_to_dll_assist_secs_Ti; double d_carr_phase_error_secs_Ti; double d_code_error_chips_Ti; - double d_preamble_timestamp_s; + double d_preamble_timestamp_samples; int32_t d_extend_correlation_ms; double d_code_error_filt_chips_s; double d_code_error_filt_chips_Ti; diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc index ebd6894e6..b137f50eb 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc @@ -83,7 +83,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_sc::msg_handler_preamble_index(const p DLOG(INFO) << "Extended correlation enabled for Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); if (d_enable_extended_integration == false) // avoid re-setting preamble indicator { - d_preamble_timestamp_s = pmt::to_double(msg); + d_preamble_timestamp_samples = pmt::to_double(msg); d_enable_extended_integration = true; d_preamble_synchronized = false; } @@ -132,7 +132,7 @@ glonass_l2_ca_dll_pll_c_aid_tracking_sc::glonass_l2_ca_dll_pll_c_aid_tracking_sc d_pll_to_dll_assist_secs_Ti(0.0), d_carr_phase_error_secs_Ti(0.0), d_code_error_chips_Ti(0.0), - d_preamble_timestamp_s(0.0), + d_preamble_timestamp_samples(0.0), d_extend_correlation_ms(extend_correlation_ms), d_code_error_filt_chips_s(0.0), d_code_error_filt_chips_Ti(0.0), @@ -152,8 +152,8 @@ glonass_l2_ca_dll_pll_c_aid_tracking_sc::glonass_l2_ca_dll_pll_c_aid_tracking_sc d_dump(dump) { // 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"), + this->message_port_register_in(pmt::mp("preamble_timestamp_samples")); + this->set_msg_handler(pmt::mp("preamble_timestamp_samples"), #if HAS_GENERIC_LAMBDA [this](auto &&PH1) { msg_handler_preamble_index(PH1); }); #else @@ -206,7 +206,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_sc::start_tracking() const double acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); // Doppler effect // Fd=(C/(C+Vr))*F - d_glonass_freq_ch = GLONASS_L2_CA_FREQ_HZ + (GLONASS_L2_CA_FREQ_HZ * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); + d_glonass_freq_ch = GLONASS_L2_CA_FREQ_HZ + (DFRQ2_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)); const double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch; // new chip and prn sequence periods based on acq Doppler d_code_freq_chips = radial_velocity * GLONASS_L2_CA_CODE_RATE_CPS; @@ -269,8 +269,8 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_sc::start_tracking() // enable tracking d_pull_in = true; d_enable_tracking = true; - d_enable_extended_integration = true; - d_preamble_synchronized = true; + d_enable_extended_integration = false; + d_preamble_synchronized = false; d_acc_carrier_phase_initialized = false; LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz @@ -601,7 +601,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at bool enable_dll_pll; if (d_enable_extended_integration == true) { - const int64_t symbol_diff = round(1000.0 * ((static_cast(d_sample_counter) + d_rem_code_phase_samples) / static_cast(d_fs_in) - d_preamble_timestamp_s)); + const int64_t symbol_diff = round(1000.0 * ((static_cast(d_sample_counter) + d_rem_code_phase_samples - d_preamble_timestamp_samples) / static_cast(d_fs_in))); if (symbol_diff > 0 and symbol_diff % d_extend_correlation_ms == 0) { // compute coherent integration and enable tracking loop @@ -663,6 +663,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at // perform basic (1ms) correlation // UPDATE INTEGRATION TIME CURRENT_INTEGRATION_TIME_S = static_cast(d_correlation_length_samples) / static_cast(d_fs_in); + d_code_loop_filter.set_pdi(static_cast(CURRENT_INTEGRATION_TIME_S)); enable_dll_pll = true; } } diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.h index 0902aa17a..ae9047f7b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.h @@ -165,7 +165,7 @@ private: double d_pll_to_dll_assist_secs_Ti; double d_carr_phase_error_secs_Ti; double d_code_error_chips_Ti; - double d_preamble_timestamp_s; + double d_preamble_timestamp_samples; int32_t d_extend_correlation_ms; double d_code_error_filt_chips_s; double d_code_error_filt_chips_Ti;