mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-16 06:44:57 +00:00
Fix hash code does not match bug in pvt to trk vtl msg. Clode vtl still does not work
This commit is contained in:
parent
e2af50f569
commit
c0c1c20eb6
@ -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
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user