mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-16 06:44:57 +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
Show Stats 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)) if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false, d_enable_vtl, d_close_vtl_loop))
{ {
// ****** experimental VTL tests // ****** 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; std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
int idx = 0; 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; 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. //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. //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++; idx++;
} }
catch (std::exception& ex) 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 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 //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; double delta_t_s = static_cast<double>(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in;
arma::mat F_tmp; 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}, 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, 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}, {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; arma::vec x_tmp;
// 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};
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 // 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 << " [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 else