diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 26d42afcb..c98eaa300 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2206,6 +2206,24 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // #### solve PVT and store the corrected observable set if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0, *d_sensor_data_aggregator)) { + if (d_internal_pvt_solver->vtl_output == true) + { + try + { + const auto& trk_cmds = d_internal_pvt_solver->vtl_Core->get_trk_cmd_outs(); + for (const auto& cmd : trk_cmds) + { + // Create shared_ptr from the TrackingCmd instance + std::shared_ptr trk_cmd_test = std::make_shared(cmd); + this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); + } + } + catch (std::exception& ex) + { + std::cout << "Error accessing VTL tracking commands: " << ex.what() << "\n"; + } + } + d_pvt_errors_counter = 0; // Reset consecutive PVT error counter const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s(); diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 73d855a1a..861af919f 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -163,10 +163,13 @@ private: bool d_flag_dump_mat_enabled; // vector tracking - std::unique_ptr vtl_data; - std::unique_ptr vtl_Core; uint32_t d_rx_clk_b_idx; uint32_t d_rx_clk_d_idx; + +public: + // vector tracking + std::unique_ptr vtl_data; + std::unique_ptr vtl_Core; int vtl_epoch; bool vtl_output; }; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 4dde2d88e..55a8d67a3 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -45,6 +45,7 @@ #include "gps_sdr_signal_replica.h" #include "lock_detectors.h" #include "tracking_discriminators.h" +#include "trackingcmd.h" #include // for io_signature #include // for scoped_lock #include // for Mat_VarCreate @@ -154,6 +155,19 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) #else boost::bind(&dll_pll_veml_tracking::msg_handler_telemetry_to_trk, this, _1)); #endif +#endif + // PVT message port input + this->message_port_register_in(pmt::mp("pvt_to_trk")); + this->set_msg_handler( + pmt::mp("pvt_to_trk"), +#if HAS_GENERIC_LAMBDA + [this](auto &&PH1) { msg_handler_pvt_to_trk(PH1); }); +#else +#if USE_BOOST_BIND_PLACEHOLDERS + boost::bind(&dll_pll_veml_tracking::msg_handler_pvt_to_trk, this, boost::placeholders::_1)); +#else + boost::bind(&dll_pll_veml_tracking::msg_handler_pvt_to_trk, this, _1)); +#endif #endif // initialize internal vars @@ -606,6 +620,11 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) d_last_timetag_samplecounter = 0; d_timetag_waiting = false; set_tag_propagation_policy(TPP_DONT); // no tag propagation, the time tag will be adjusted and regenerated in work() + + d_VTL_pll_en = false; + d_VTL_dll_en = false; + d_prn_id = 0; + d_VTL_dll_weight_shift = 0; } @@ -645,6 +664,68 @@ void dll_pll_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) } +void dll_pll_veml_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) +{ + try + { + if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + const auto cmd = wht::any_cast>(pmt::any_ref(msg)); + if (cmd->channel_id == this->d_channel) + { + gr::thread::scoped_lock lock(d_setlock); + d_prn_id = cmd->prn_id; + d_sample_counter_VTL = cmd->ch_sample_counter; + + // PLL + d_carr_freq_hz_VTL = cmd->pll_vtl_freq_hz; + d_carr_freq_hz_s_VTL = cmd->carrier_freq_rate_hz_s; + if (d_VTL_pll_en && (cmd->enable_pll_vtl_feedack == false)) + { + d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); + } + d_VTL_pll_en = cmd->enable_pll_vtl_feedack; + + // DLL + d_code_freq_hz_VTL = cmd->dll_vtl_freq_hz; + if (d_signal_type == "1B" || d_signal_type == "1C") + { + d_code_freq_hz_s_VTL = cmd->carrier_freq_rate_hz_s / GPS_L1_FREQ_HZ / GPS_L1_CA_CODE_RATE_CPS; + } + else + { + d_code_freq_hz_s_VTL = cmd->carrier_freq_rate_hz_s / GPS_L5_FREQ_HZ / GPS_L5I_CODE_RATE_CPS; + } + + + d_G_vtl = 1; // start with 100% weight to VTL feedback + if (d_signal_type == "1B") + { + d_VTL_dll_weight_shift = 0.2; // shift 20% to traditional tracking each epoch + } + else + { + d_VTL_dll_weight_shift = 0.05; // shift 5% + } + if (d_VTL_dll_en && (cmd->enable_dll_vtl_feedack == false)) + { + d_code_loop_filter.initialize(); + } + d_VTL_dll_en = cmd->enable_dll_vtl_feedack; + } + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const wht::bad_any_cast &e) + { + LOG(WARNING) << "msg_handler_pvt_to_trk Bad any_cast: " << e.what(); + } +} + + void dll_pll_veml_tracking::start_tracking() { gr::thread::scoped_lock l(d_setlock); @@ -1111,6 +1192,13 @@ void dll_pll_veml_tracking::run_dll_pll() // New carrier Doppler frequency estimation d_carrier_doppler_hz = d_carr_error_filt_hz; + // VPLL + if (d_VTL_pll_en) + { + double delta_t_s = static_cast(this->nitems_read(0) - d_sample_counter_VTL) / d_trk_parameters.fs_in; + d_carrier_doppler_hz = d_carr_freq_hz_VTL + delta_t_s * d_carr_freq_hz_s_VTL; + } + // std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << '\n'; // std::cout << "d_CN0_SNV_dB_Hz: " << this->d_CN0_SNV_dB_Hz << '\n'; @@ -1133,6 +1221,18 @@ void dll_pll_veml_tracking::run_dll_pll() d_code_freq_chips += d_carrier_doppler_hz * d_code_chip_rate / d_signal_carrier_freq; } + // VDLL + if (d_VTL_dll_en) + { + double delta_t_s = static_cast(this->nitems_read(0) - d_sample_counter_VTL) / d_trk_parameters.fs_in; + d_code_freq_chips = d_G_vtl * (d_code_freq_hz_VTL + delta_t_s * d_code_freq_hz_s_VTL) + (1 - d_G_vtl) * d_code_freq_chips; + + if (d_G_vtl > 0) + { + d_G_vtl -= d_VTL_dll_weight_shift; + } + } + // Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler if (d_trk_parameters.enable_doppler_correction == true) { diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 0c0856ee2..3fdb89bf1 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -75,6 +75,7 @@ private: explicit dll_pll_veml_tracking(const Dll_Pll_Conf &conf_); void msg_handler_telemetry_to_trk(const pmt::pmt_t &msg); + void msg_handler_pvt_to_trk(const pmt::pmt_t &msg); void do_correlation_step(const gr_complex *input_samples); void run_dll_pll(); void check_carrier_phase_coherent_initialization(); @@ -126,6 +127,12 @@ private: double d_code_error_chips; double d_code_error_filt_chips; double d_code_freq_chips; + double d_carr_freq_hz_VTL; + double d_carr_freq_hz_s_VTL; + double d_code_freq_hz_VTL; + double d_code_freq_hz_s_VTL; + double d_sample_counter_VTL; + double d_G_vtl; double d_carrier_doppler_hz; double d_acc_carrier_phase_rad; double d_rem_code_phase_chips; @@ -204,6 +211,12 @@ private: bool d_acc_carrier_phase_initialized; bool d_enable_extended_integration; bool d_Flag_PLL_180_deg_phase_locked; + + // vector tracking + bool d_VTL_pll_en; + bool d_VTL_dll_en; + uint32_t d_prn_id; + float d_VTL_dll_weight_shift; };