From 9b210485c73c3214df0c024196c3081526142757 Mon Sep 17 00:00:00 2001 From: Vladisslav P Date: Tue, 17 May 2022 13:40:30 +0300 Subject: [PATCH] Fix GLONASS extended correlator 1. Emit a message from preamble detection branch from telemetry decoder 2. Connect flowgraph message ports to pass the message from telemetry decoder block to a dll-pll-c-aid-tracking block 3. Name message ports correctly as the massage is passed in samples, not seconds Signed-off-by: Vladisslav P --- src/algorithms/channel/adapters/channel.cc | 8 ++++++++ .../glonass_l1_ca_telemetry_decoder_gs.cc | 2 ++ .../glonass_l2_ca_telemetry_decoder_gs.cc | 2 ++ .../glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc | 10 +++++----- .../glonass_l1_ca_dll_pll_c_aid_tracking_cc.h | 2 +- .../glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc | 10 +++++----- .../glonass_l2_ca_dll_pll_c_aid_tracking_cc.h | 2 +- 7 files changed, 24 insertions(+), 12 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index feac903cd..28376dfd8 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -128,6 +128,10 @@ 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")) + { + top_block->msg_connect(nav_->get_left_block(), pmt::mp("preamble_timestamp_samples"), trk_->get_right_block(), pmt::mp("preamble_timestamp_samples")); + } DLOG(INFO) << "tracking -> telemetry_decoder"; // Message ports @@ -158,6 +162,10 @@ 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")) + { + top_block->msg_disconnect(nav_->get_left_block(), pmt::mp("preamble_timestamp_samples"), trk_->get_right_block(), pmt::mp("preamble_timestamp_samples")); + } if (!flag_enable_fpga_) { top_block->msg_disconnect(acq_->get_right_block(), pmt::mp("events"), channel_msg_rx_, pmt::mp("events")); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc index 0ebb91bb4..ef9964a4a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc @@ -69,6 +69,7 @@ glonass_l1_ca_telemetry_decoder_gs::glonass_l1_ca_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block this->message_port_register_out(pmt::mp("telemetry_to_trk")); + this->message_port_register_out(pmt::mp("preamble_timestamp_samples")); if (d_enable_navdata_monitor) { @@ -367,6 +368,7 @@ int glonass_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu // try to decode frame LOG(INFO) << "Starting string decoder for GLONASS L1 C/A SAT " << this->d_satellite; d_preamble_index = d_sample_counter; // record the preamble sample stamp + this->message_port_pub(pmt::mp("preamble_timestamp_samples"), pmt::mp(d_preamble_time_samples)); d_stat = 2; } else diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc index aa1707e93..b0a6d8812 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc @@ -69,6 +69,7 @@ glonass_l2_ca_telemetry_decoder_gs::glonass_l2_ca_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block this->message_port_register_out(pmt::mp("telemetry_to_trk")); + this->message_port_register_out(pmt::mp("preamble_timestamp_samples")); if (d_enable_navdata_monitor) { @@ -366,6 +367,7 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu // try to decode frame LOG(INFO) << "Starting string decoder for GLONASS L2 C/A SAT " << this->d_satellite; d_preamble_index = d_sample_counter; // record the preamble sample stamp + this->message_port_pub(pmt::mp("preamble_timestamp_samples"), pmt::mp(d_preamble_time_samples)); d_stat = 2; } else diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc index ba6a7c0e9..05b8eb578 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -87,7 +87,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::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; } @@ -138,7 +138,7 @@ glonass_l1_ca_dll_pll_c_aid_tracking_cc::glonass_l1_ca_dll_pll_c_aid_tracking_cc d_code_error_filt_chips_s(0.0), d_code_error_filt_chips_Ti(0.0), d_carr_phase_error_secs_Ti(0.0), - d_preamble_timestamp_s(0.0), + d_preamble_timestamp_samples(0.0), d_extend_correlation_ms(extend_correlation_ms), d_correlation_length_samples(static_cast(d_vector_length)), d_sample_counter(0ULL), @@ -156,9 +156,9 @@ glonass_l1_ca_dll_pll_c_aid_tracking_cc::glonass_l1_ca_dll_pll_c_aid_tracking_cc d_dump(dump) { // Telemetry bit synchronization message port input - this->message_port_register_in(pmt::mp("preamble_timestamp_s")); + this->message_port_register_in(pmt::mp("preamble_timestamp_samples")); - this->set_msg_handler(pmt::mp("preamble_timestamp_s"), + this->set_msg_handler(pmt::mp("preamble_timestamp_samples"), #if HAS_GENERIC_LAMBDA [this](auto &&PH1) { msg_handler_preamble_index(PH1); }); #else @@ -602,7 +602,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::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 diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h index a11fb2a3f..97d05ee93 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h @@ -171,7 +171,7 @@ private: double d_code_error_filt_chips_Ti; double d_carr_phase_error_secs_Ti; - double d_preamble_timestamp_s; + double d_preamble_timestamp_samples; int32_t d_extend_correlation_ms; // Integration period in samples diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc index 44a4bdcbd..74bc090ff 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc @@ -84,7 +84,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_cc::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; } @@ -135,7 +135,7 @@ glonass_l2_ca_dll_pll_c_aid_tracking_cc::glonass_l2_ca_dll_pll_c_aid_tracking_cc d_code_error_filt_chips_s(0.0), d_code_error_filt_chips_Ti(0.0), d_carr_phase_error_secs_Ti(0.0), - d_preamble_timestamp_s(0.0), + d_preamble_timestamp_samples(0.0), d_extend_correlation_ms(extend_correlation_ms), d_correlation_length_samples(static_cast(d_vector_length)), d_sample_counter(0ULL), @@ -153,9 +153,9 @@ glonass_l2_ca_dll_pll_c_aid_tracking_cc::glonass_l2_ca_dll_pll_c_aid_tracking_cc d_dump(dump) { // Telemetry bit synchronization message port input - this->message_port_register_in(pmt::mp("preamble_timestamp_s")); + this->message_port_register_in(pmt::mp("preamble_timestamp_samples")); - this->set_msg_handler(pmt::mp("preamble_timestamp_s"), + this->set_msg_handler(pmt::mp("preamble_timestamp_samples"), #if HAS_GENERIC_LAMBDA [this](auto &&PH1) { msg_handler_preamble_index(PH1); }); #else @@ -600,7 +600,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::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 diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h index bebb30333..dd92858e5 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h @@ -168,7 +168,7 @@ private: double d_code_error_filt_chips_s; double d_code_error_filt_chips_Ti; double d_carr_phase_error_secs_Ti; - double d_preamble_timestamp_s; + double d_preamble_timestamp_samples; int32_t d_extend_correlation_ms; // Integration period in samples