From 85debfea6d7678a483e042ff1c24984a0b18d2bd Mon Sep 17 00:00:00 2001 From: pedromiguelcp Date: Tue, 5 Aug 2025 14:54:57 +0100 Subject: [PATCH] style: improve code readability --- src/algorithms/PVT/libs/rtklib_solver.cc | 2 +- src/algorithms/PVT/libs/vtl_core.cc | 7 +++-- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 27 ++++++++++--------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 4 +-- 4 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 5d4d3af02..053ffe734 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1677,7 +1677,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ vtl_data->obs_prr(ch_id) = -gnss_observables_iter->second.Carrier_Doppler_hz * Lambda_GPS_L1; vtl_data->band(ch_id) = 0; // L1E1 } - else + else if (std::string(gnss_observables_iter->second.Signal) == "L5" || std::string(gnss_observables_iter->second.Signal) == "5X") { vtl_data->obs_prr(ch_id) = -gnss_observables_iter->second.Carrier_Doppler_hz * Lambda_GPS_L5; vtl_data->band(ch_id) = 1; // L5E5 diff --git a/src/algorithms/PVT/libs/vtl_core.cc b/src/algorithms/PVT/libs/vtl_core.cc index 1e14fcef2..650ff13ee 100755 --- a/src/algorithms/PVT/libs/vtl_core.cc +++ b/src/algorithms/PVT/libs/vtl_core.cc @@ -277,16 +277,15 @@ void Vtl_Core::update_process(double dt_s) const double q_pv = vtl_sys_acc_noise_var_m2s4 * T3 / 2; const double q_vv = vtl_sys_acc_noise_var_m2s4 * T2; - // PVA arma::mat Q_PV(2, 2, arma::fill::zeros); Q_PV(i_px, i_px) = q_pp; Q_PV(i_px, i_vx) = q_pv; Q_PV(i_vx, i_px) = q_pv; Q_PV(i_vx, i_vx) = q_vv; // 3 motion axes (x, y, z) - ekf_Q.submat(i_px, i_px, i_vx, i_vx) = vtl_kinematic ? Q_PV : arma::zeros(2, 2); - ekf_Q.submat(i_py, i_py, i_vy, i_vy) = vtl_kinematic ? Q_PV : arma::zeros(2, 2); - ekf_Q.submat(i_pz, i_pz, i_vz, i_vz) = vtl_kinematic ? Q_PV : arma::zeros(2, 2); + ekf_Q.submat(i_px, i_px, i_vx, i_vx) = vtl_kinematic ? Q_PV : (arma::ones(2, 2) * 1e-10); + ekf_Q.submat(i_py, i_py, i_vy, i_vy) = vtl_kinematic ? Q_PV : (arma::ones(2, 2) * 1e-10); + ekf_Q.submat(i_pz, i_pz, i_vz, i_vz) = vtl_kinematic ? Q_PV : (arma::ones(2, 2) * 1e-10); // Clock const double clk_b_noise_var_m2 = S_f + vtl_sys_clk_b_noise_var_m2; 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 55a8d67a3..2b9e603b9 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -624,7 +624,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) d_VTL_pll_en = false; d_VTL_dll_en = false; d_prn_id = 0; - d_VTL_dll_weight_shift = 0; + d_W_shift_vtl = 0; } @@ -688,25 +688,26 @@ void dll_pll_veml_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) // DLL d_code_freq_hz_VTL = cmd->dll_vtl_freq_hz; - if (d_signal_type == "1B" || d_signal_type == "1C") + if (d_signal_type == "1C" || d_signal_type == "1B") { d_code_freq_hz_s_VTL = cmd->carrier_freq_rate_hz_s / GPS_L1_FREQ_HZ / GPS_L1_CA_CODE_RATE_CPS; } - else + else if (d_signal_type == "L5" || d_signal_type == "5X") { 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 + d_W_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 + d_W_shift_vtl = 0.2; // shift 20% to traditional tracking each epoch } - else + else if (d_signal_type == "1C" || d_signal_type == "L5" || d_signal_type == "5X") { - d_VTL_dll_weight_shift = 0.05; // shift 5% + d_W_shift_vtl = 0.05; // shift 5% } + if (d_VTL_dll_en && (cmd->enable_dll_vtl_feedack == false)) { d_code_loop_filter.initialize(); @@ -1196,7 +1197,7 @@ void dll_pll_veml_tracking::run_dll_pll() 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; + d_carrier_doppler_hz = d_W_vtl * (d_carr_freq_hz_VTL + delta_t_s * d_carr_freq_hz_s_VTL) + (1 - d_W_vtl) * d_carrier_doppler_hz; } // std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << '\n'; @@ -1225,12 +1226,12 @@ void dll_pll_veml_tracking::run_dll_pll() 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; + d_code_freq_chips = d_W_vtl * (d_code_freq_hz_VTL + delta_t_s * d_code_freq_hz_s_VTL) + (1 - d_W_vtl) * d_code_freq_chips; + } - if (d_G_vtl > 0) - { - d_G_vtl -= d_VTL_dll_weight_shift; - } + if (d_W_vtl > 0) + { + d_W_vtl -= d_W_shift_vtl; } // Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler 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 3fdb89bf1..fff7d766d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -132,7 +132,7 @@ private: double d_code_freq_hz_VTL; double d_code_freq_hz_s_VTL; double d_sample_counter_VTL; - double d_G_vtl; + double d_W_vtl; double d_carrier_doppler_hz; double d_acc_carrier_phase_rad; double d_rem_code_phase_chips; @@ -216,7 +216,7 @@ private: bool d_VTL_pll_en; bool d_VTL_dll_en; uint32_t d_prn_id; - float d_VTL_dll_weight_shift; + float d_W_shift_vtl; };