From b68d8c5c7c329e15e614dfc5a59c7a9ce308c95a Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 24 May 2023 17:23:25 +0200 Subject: [PATCH] FIX: doppler feedback --- src/algorithms/PVT/libs/vtl_engine.cc | 2 +- .../tracking/gnuradio_blocks/kf_tracking.cc | 41 +++---------------- 2 files changed, 7 insertions(+), 36 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 80e5c6165..6c0b28f0e 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -111,7 +111,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { kf_R(i, i) = 80.0; kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0; - kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 400.0; + kf_R(i + 2 * new_data.sat_number, i + 2 * new_data.sat_number) = 40.0; if (i == 6) { diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index ec233df9f..00095ac12 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -657,46 +657,17 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) double old_code_phase_chips = d_x_old_old(0); if (cmd->enable_carrier_nco_cmd) - { - if (abs(d_x_old_old(2) - tmp_x(2)) > 50) - { - // std::cout << "channel: " << this->d_channel - // << " tracking_cmd TOO FAR: " - // << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" - // << " \n"; - } - else - { - // std::cout << "channel: " << this->d_channel - // << " tracking_cmd NEAR: " - // << abs(d_x_old_old(2) - tmp_x(2)) << "Hz" - // << " \n"; - } - d_x_old_old(2) = tmp_x(2); // replace DOPPLER - d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE - } + { + d_x_old_old(2) = tmp_x(2); // replace DOPPLER + } else - { - // std::cout << "correction not applied" << std::endl; - } + { + // std::cout << "correction not applied" << std::endl; + } // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true; d_vtl_cmd_samplestamp = cmd->sample_counter; - // std::cout << "CH " << this->d_channel << " RX pvt-to-trk cmd with delay: " - // << static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in - // << " SampleCounter origin: " << cmd->sample_counter - // << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" - // << " [s]\n"; - // if(cmd->channel_id ==0) - // { - // std::cout << "CH " << cmd->channel_id << " RX pvt-to-trk cmd with delay: " - // << delta_t_s << "[s]" - // << " SampleCounter origin: " << cmd->sample_counter - // << " code phase new state: " << x_tmp(0) << " vs. trk state: " << old_code_phase_chips << " [chips]" - // << "\n"; - // std::cout << "use count " <