mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-09-10 14:56:03 +00:00
Code Issues Releases Wiki Activity

feat: add vector tracking feedback to tracking loop

Browse Source
This commit is contained in:
pedromiguelcp
2025-07-02 18:33:01 +01:00
committed by Carles Fernandez
parent 04cb04ef28
commit 7c52044a25
4 changed files with 136 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@@ -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();

7
src/algorithms/PVT/libs/rtklib_solver.h
View File

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

100
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc
View File

@@ -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)
{

13
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h
View File

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