mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-11 01:41:01 +00:00
Code Issues Releases Wiki Activity

MOD: debug in kf_tracking

Browse Source
This commit is contained in:
M.A.Gomez 2022-12-17 12:46:40 +00:00
parent 54c3d6f964
commit e3a62735e3
1 changed files with 28 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
View File

@ -57,6 +57,8 @@
#include <map> #include <map>
#include <numeric> #include <numeric>
#include <vector> #include <vector>
#include "iostream"
#include <fstream>
#if HAS_GENERIC_LAMBDA #if HAS_GENERIC_LAMBDA
#else #else
@ -647,18 +649,40 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
// states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s
x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s}; x_tmp = {cmd->code_phase_chips, cmd->carrier_phase_rads, cmd->carrier_freq_hz, cmd->carrier_freq_rate_hz_s};
// TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz) // TODO: Replace only the desired states and leave the others as stored in d_x_old_old vector (e.g replace only the carrier_freq_hz)
arma::vec tmp_x = F_tmp * x_tmp; //arma::vec tmp_x = F_tmp * x_tmp;
double old_doppler = d_x_old_old(2); double old_doppler = d_x_old_old(2);
d_x_old_old(2) = tmp_x(2); //replace only the Carrier Frequency state d_x_old_old(2) = x_tmp(2); //replace only the Carrier Frequency state
// set vtl corrections flag to inform VTL from gnss_synchro object // set vtl corrections flag to inform VTL from gnss_synchro object
d_vtl_cmd_applied_now = true; d_vtl_cmd_applied_now = true;
d_vtl_cmd_samplestamp = cmd->sample_counter; d_vtl_cmd_samplestamp = cmd->sample_counter;
std::cout << "CH " << this->d_channel << " RX pvt-to-trk cmd with delay: " // std::cout << "CH " << this->d_channel << " RX pvt-to-trk cmd with delay: "
// << static_cast<double>(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(this->d_channel ==0)
{
std::cout << "CH " << this->d_channel << " RX pvt-to-trk cmd with delay: "
<< static_cast<double>(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in << static_cast<double>(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in
<< " SampleCounter origin: " << cmd->sample_counter << " SampleCounter origin: " << cmd->sample_counter
<< " Doppler new state: " << tmp_x(2) << " vs. trk state: " << old_doppler << " [Hz]" << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]"
<< " [s]\n"; << " [s]\n";
}
std::fstream dump_tracking_file;
dump_tracking_file.open("dump_trk_file.csv", std::ios::out | std::ios::app);
dump_tracking_file.precision(15);
if (!dump_tracking_file)
{
std::cout << "dump_tracking_file not created!";
}
else
{
dump_tracking_file << "doppler_corr"
<< ","<< this->d_channel << "," << x_tmp(2) << "," << old_doppler << "\n";
dump_tracking_file.close();
}
} }
} }
else else