From c0c1c20eb6bafa26e2b41229f1f97465e33183da Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Fri, 16 Dec 2022 19:05:10 +0100 Subject: [PATCH] Fix hash code does not match bug in pvt to trk vtl msg. Clode vtl still does not work --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 9 +++++++-- .../tracking/gnuradio_blocks/kf_tracking.cc | 11 +++++++++-- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index fb60ade51..69f08ee21 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2136,7 +2136,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false, d_enable_vtl, d_close_vtl_loop)) { // ****** experimental VTL tests - if (d_close_vtl_loop == true) + if (d_close_vtl_loop == true and d_enable_vtl == true) { std::map::const_iterator gnss_observables_iter; int idx = 0; @@ -2149,7 +2149,8 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item d_internal_pvt_solver->vtl_engine.trk_cmd_outs.at(idx).channel_id = gnss_observables_iter->second.Channel_ID; //todo: VTL loop CAN NOT run every PVT epoch because it is required to wait for the corrections to be applied to the tracking KF. //currently the VTL runs every PVT epoch which will create inestabilities. - this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(d_internal_pvt_solver->vtl_engine.trk_cmd_outs.at(idx))); + const std::shared_ptr trk_cmd_test = std::make_shared(d_internal_pvt_solver->vtl_engine.trk_cmd_outs.at(idx)); + this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); idx++; } catch (std::exception& ex) @@ -2202,6 +2203,10 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // } // } } + else + { + std::cout << "Loop open!\n"; + } // ***************************** d_pvt_errors_counter = 0; // Reset consecutive PVT error counter diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 58ac6c391..fa9c9d058 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -638,6 +638,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) //To.Do: apply VTL corrections to the KF states double delta_t_s = static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in; arma::mat F_tmp; + //ToDO: check state propagation, at least Doppler propagation does NOT work, see debug traces F_tmp = {{1.0, 0.0, d_beta * delta_t_s, d_beta * (delta_t_s * delta_t_s) / 2.0}, {0.0, 1.0, 2.0 * GNSS_PI * delta_t_s, GNSS_PI * (delta_t_s * delta_t_s)}, {0.0, 0.0, 1.0, delta_t_s}, @@ -645,13 +646,19 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) arma::vec x_tmp; // 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}; - d_x_old_old = F_tmp * x_tmp; + // 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; + double old_doppler = d_x_old_old(2); + d_x_old_old(2) = tmp_x(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: " - << static_cast(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n"; + << 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]" + << " [s]\n"; } } else