diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index fa9c9d058..5e0f4d2e8 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -57,6 +57,8 @@ #include #include #include +#include "iostream" +#include #if HAS_GENERIC_LAMBDA #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 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) - arma::vec tmp_x = F_tmp * x_tmp; + //arma::vec tmp_x = F_tmp * x_tmp; 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 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: " + // 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(this->d_channel ==0) + { + 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: " << tmp_x(2) << " vs. trk state: " << old_doppler << " [Hz]" + << " Doppler new state: " << x_tmp(2) << " vs. trk state: " << old_doppler << " [Hz]" << " [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