diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc index de01987b7..98f1433a8 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc @@ -58,6 +58,7 @@ #include #include #include +#include #include #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(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(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(d_dll_filt_history.capacity()); + if (fabs(avg_code_error_chips_s) > 1.0) + { + float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(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(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)) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 5eabb27a9..850786cdf 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/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 d_symbol_history; + // dll filter buffer + boost::circular_buffer 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; diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc b/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc index 88c6ead1d..74be48fda 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.cc +++ b/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"; diff --git a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h b/src/algorithms/tracking/libs/dll_pll_conf_fpga.h index c61066e84..48acbe624 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf_fpga.h +++ b/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;