added the bit synchronization time limit and the doppler correction

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc

@@ -58,6 +58,7 @@
 #include <exception>
 #include <iostream>
 #include <map>
+#include <numeric>
 #include <vector>
 
 #if HAS_STD_FILESYSTEM
@@ -96,6 +97,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga &
     this->set_msg_handler(pmt::mp("telemetry_to_trk"), boost::bind(&dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk, this, _1));
 
     // initialize internal vars
+    d_dll_filt_history.set_capacity(2000);
     d_veml = false;
     d_cloop = true;
     d_pull_in_transitory = true;
@@ -563,11 +565,15 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga &
     d_num_current_syncrho_repetitions = 1;
 
 
+    d_corrected_doppler = false;
+
     // debug - erase previous outfile contents
     std::ofstream outfile;
     outfile.open("trk_out.txt", std::ios_base::trunc);
     outfile.close();
     d_worker_is_done = false;
+
+
 }
 
 void dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg)
@@ -623,6 +629,8 @@ void dll_pll_veml_tracking_fpga::start_tracking()
     d_carrier_loop_filter.initialize(static_cast<float>(d_acq_carrier_doppler_hz));  // initialize the carrier filter
 
 
+    d_corrected_doppler = false;
+
     boost::mutex::scoped_lock lock(d_mutex);
     d_worker_is_done = true;
     m_condition.notify_one();
@@ -847,6 +855,29 @@ void dll_pll_veml_tracking_fpga::run_dll_pll()
 
     // New code Doppler frequency estimation
     d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate - d_code_error_filt_chips;
+
+    // Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler
+    if (trk_parameters.enable_doppler_correction == true)
+        {
+            if (d_pull_in_transitory == false and d_corrected_doppler == false)
+                {
+                    d_dll_filt_history.push_back(static_cast<float>(d_code_error_filt_chips));
+
+                    if (d_dll_filt_history.full())
+                        {
+                            float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast<float>(d_dll_filt_history.capacity());
+                            if (fabs(avg_code_error_chips_s) > 1.0)
+                                {
+                                    float carrier_doppler_error_hz = static_cast<float>(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast<float>(d_code_chip_rate);
+                                    LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN);
+                                    d_carrier_loop_filter.initialize(d_carrier_doppler_hz - carrier_doppler_error_hz);
+                                    d_corrected_doppler = true;
+                                }
+                            d_dll_filt_history.clear();
+                        }
+                }
+        }
+
 }
 
 
@@ -1974,6 +2005,14 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
                 d_P_accu = *d_Prompt;
                 d_L_accu = *d_Late;
 
+                //fail-safe: check if the secondary code or bit synchronization has not succedded in a limited time period
+                if (trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast<int>(trk_parameters.fs_in))
+                    {
+                        d_carrier_lock_fail_counter = 300000;  //force loss-of-lock condition
+                        LOG(INFO) << systemName << " " << signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel
+                                  << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
+                    }
+
                 // Check lock status
 
                 if (!cn0_and_tracking_lock_status(d_code_period))

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h

@@ -124,6 +124,8 @@ private:
     int32_t d_preamble_length_symbols;
     boost::circular_buffer<float> d_symbol_history;
 
+    // dll filter buffer
+    boost::circular_buffer<float> d_dll_filt_history;
     // tracking state machine
     int32_t d_state;
 
@@ -180,6 +182,7 @@ private:
 
     // tracking vars
     bool d_pull_in_transitory;
+    bool d_corrected_doppler;
     double d_current_correlation_time_s;
     double d_carr_phase_error_hz;
     double d_carr_freq_error_hz;

src/algorithms/tracking/libs/dll_pll_conf_fpga.cc

@@ -46,7 +46,8 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga()
     dump_filename = std::string("./dll_pll_dump.dat");
     enable_fll_pull_in = false;
     enable_fll_steady_state = false;
-    pull_in_time_s = 2;
+    pull_in_time_s = 10;
+    bit_synchronization_time_limit_s = pull_in_time_s + 60;
     fll_filter_order = 1;
     pll_filter_order = 3;
     dll_filter_order = 2;
@@ -66,6 +67,7 @@ Dll_Pll_Conf_Fpga::Dll_Pll_Conf_Fpga()
     cn0_min = 25;
     max_lock_fail = 50;
     carrier_lock_th = 0.85;
+    enable_doppler_correction = false;
     track_pilot = false;
     system = 'G';
     char sig_[3] = "1C";

src/algorithms/tracking/libs/dll_pll_conf_fpga.h

@@ -46,6 +46,7 @@ public:
     bool enable_fll_pull_in;
     bool enable_fll_steady_state;
     unsigned int pull_in_time_s;  // signed integer, when pull in time is not yet reached it has to be compared against a negative number
+    unsigned int bit_synchronization_time_limit_s;
     int pll_filter_order;
     int dll_filter_order;
 
@@ -74,6 +75,7 @@ public:
     uint32_t smoother_length;
     double carrier_lock_th;
     bool track_pilot;
+    bool enable_doppler_correction;
     char system;
     char signal[3];
     std::string device_name;