feat: add vector tracking feedback to tracking loop

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<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(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();
 

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> vtl_data;
-    std::unique_ptr<Vtl_Core> vtl_Core;
     uint32_t d_rx_clk_b_idx;
     uint32_t d_rx_clk_d_idx;
+
+public:
+    // vector tracking
+    std::unique_ptr<Vtl_Data> vtl_data;
+    std::unique_ptr<Vtl_Core> vtl_Core;
     int vtl_epoch;
     bool vtl_output;
 };

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 <gnuradio/io_signature.h>   // for io_signature
 #include <gnuradio/thread/thread.h>  // for scoped_lock
 #include <matio.h>                   // 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<TrackingCmd>).hash_code())
+                {
+                    const auto cmd = wht::any_cast<const std::shared_ptr<TrackingCmd>>(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<double>(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<double>(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)
         {

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