mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-31 07:13:03 +00:00
Code Issues Releases Wiki Activity

Fix hash code does not match bug in pvt to trk vtl msg. Clode vtl still does not work

Browse Source
This commit is contained in:
Javier Arribas
2022-12-16 19:05:10 +01:00
parent e2af50f569
commit c0c1c20eb6
2 changed files with 16 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@@ -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<int, Gnss_Synchro>::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<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(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

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

@@ -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<double>(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<double>(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n";
<< static_cast<double>(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