Merge branch 'glonass-ext-correlator-fix2' of https://github.com/vladisslav2011/gnss-sdr into vladisslav2011-glonass-ext-correlator-fix2

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;
+}

src/algorithms/channel/adapters/channel.h

@@ -96,6 +96,7 @@ public:
     inline std::shared_ptr<TelemetryDecoderInterface> telemetry() const { return nav_; }
 
 private:
+    bool glonass_dll_pll_c_aid_tracking_check();
     std::shared_ptr<ChannelFsm> channel_fsm_;
     std::shared_ptr<AcquisitionInterface> acq_;
     std::shared_ptr<TrackingInterface> trk_;
@@ -109,6 +110,7 @@ private:
     bool connected_;
     bool repeat_;
     bool flag_enable_fpga_;
+    int glonass_extend_correlation_ms_;
 };
 
 

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->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
@@ -209,7 +209,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking()
     const double acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(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_enable_extended_integration = false;
-    d_preamble_synchronized = true;
+    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<double>(d_sample_counter) + d_rem_code_phase_samples) / static_cast<double>(d_fs_in) - d_preamble_timestamp_s));
+                    int64_t symbol_diff = round(1000.0 * ((static_cast<double>(d_sample_counter) + d_rem_code_phase_samples - d_preamble_timestamp_samples) / static_cast<double>(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<double>(d_extend_correlation_ms) * GLONASS_L1_CA_CODE_PERIOD_S;
+                            d_code_loop_filter.set_pdi(static_cast<float>(CURRENT_INTEGRATION_TIME_S));
                             enable_dll_pll = true;
                         }
                     else

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;

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->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
@@ -206,7 +206,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_sc::start_tracking()
     const double acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(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_enable_extended_integration = false;
-    d_preamble_synchronized = true;
+    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<double>(d_sample_counter) + d_rem_code_phase_samples) / static_cast<double>(d_fs_in) - d_preamble_timestamp_s));
+                    const int64_t symbol_diff = round(1000.0 * ((static_cast<double>(d_sample_counter) + d_rem_code_phase_samples - d_preamble_timestamp_samples) / static_cast<double>(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<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
+                                    d_code_loop_filter.set_pdi(static_cast<float>(CURRENT_INTEGRATION_TIME_S));
                                     enable_dll_pll = true;
                                 }
                         }

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;