mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-25 08:26:59 +00:00
Code Issues Releases Wiki Activity

VTL Engine now sends messages to tracking loops

Browse Source
This commit is contained in:
Javier Arribas 2022-12-12 15:11:45 +01:00
parent c38f9bb81e
commit 8de42f8885
1 changed files with 53 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2139,48 +2139,68 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
if (d_close_vtl_loop == true)
{
std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
int idx = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin();
gnss_observables_iter != d_gnss_observables_map.cend();
++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system
{
// test complete loop
if (gnss_observables_iter->second.last_vtl_cmd_sample_counter == 0)
try
{
// send new tracking command
const std::shared_ptr<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(TrackingCmd());
trk_cmd_test->carrier_freq_hz = 12345.4;
trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter;
trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID;
this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test));
d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter;
std::cout << "msg pvt_to_trk sent.\n";
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)));
idx++;
}
else
catch (std::exception& ex)
{
// std::cout << "CH " << gnss_observables_iter->second.Channel_ID
// << " T_RX: " << static_cast<float>(gnss_observables_iter->second.Tracking_sample_counter) / static_cast<float>(gnss_observables_iter->second.fs)
// << " T_last_vtl_trk: " << static_cast<float>(gnss_observables_iter->second.last_vtl_cmd_sample_counter) / static_cast<float>(gnss_observables_iter->second.fs)
// << " T_map: " << static_cast<float>(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) / static_cast<float>(gnss_observables_iter->second.fs)
// << " T2: " << static_cast<float>(gnss_observables_iter->second.last_vtl_cmd_sample_counter) - static_cast<float>(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])
// << " T3: " << static_cast<float>(gnss_observables_iter->second.Tracking_sample_counter) - static_cast<float>(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) << "\n";
// To.Do: check if satellite change, check if there is a possibility to not find the last cmd timestamp in the map...
if (gnss_observables_iter->second.last_vtl_cmd_sample_counter >= d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])
{
std::cout << "CH " << gnss_observables_iter->second.Channel_ID << " processed VTL cmd, total loop time is "
<< ((static_cast<float>(gnss_observables_iter->second.Tracking_sample_counter) - static_cast<float>(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])) / static_cast<float>(gnss_observables_iter->second.fs)) * 1000.0
<< " [ms]!\n";
// send new tracking command
const std::shared_ptr<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(TrackingCmd());
trk_cmd_test->carrier_freq_hz = 12345.4;
trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter;
trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID;
this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test));
d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter;
std::cout << "msg pvt_to_trk sent.\n";
}
std::cout << "VTL Engine problem: " << ex.what() << "\n";
}
}
//Dummy messages for evaluation of msg latency
// std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
// for (gnss_observables_iter = d_gnss_observables_map.cbegin();
// gnss_observables_iter != d_gnss_observables_map.cend();
// ++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system
// {
// // test complete loop
// if (gnss_observables_iter->second.last_vtl_cmd_sample_counter == 0)
// {
// // send new tracking command
// const std::shared_ptr<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(TrackingCmd());
// trk_cmd_test->carrier_freq_hz = 12345.4;
// trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter;
// trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID;
// this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test));
// d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter;
// std::cout << "msg pvt_to_trk sent.\n";
// }
// else
// {
// // std::cout << "CH " << gnss_observables_iter->second.Channel_ID
// // << " T_RX: " << static_cast<float>(gnss_observables_iter->second.Tracking_sample_counter) / static_cast<float>(gnss_observables_iter->second.fs)
// // << " T_last_vtl_trk: " << static_cast<float>(gnss_observables_iter->second.last_vtl_cmd_sample_counter) / static_cast<float>(gnss_observables_iter->second.fs)
// // << " T_map: " << static_cast<float>(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) / static_cast<float>(gnss_observables_iter->second.fs)
// // << " T2: " << static_cast<float>(gnss_observables_iter->second.last_vtl_cmd_sample_counter) - static_cast<float>(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])
// // << " T3: " << static_cast<float>(gnss_observables_iter->second.Tracking_sample_counter) - static_cast<float>(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) << "\n";
//
// // To.Do: check if satellite change, check if there is a possibility to not find the last cmd timestamp in the map...
// if (gnss_observables_iter->second.last_vtl_cmd_sample_counter >= d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])
// {
// std::cout << "CH " << gnss_observables_iter->second.Channel_ID << " processed VTL cmd, total loop time is "
// << ((static_cast<float>(gnss_observables_iter->second.Tracking_sample_counter) - static_cast<float>(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])) / static_cast<float>(gnss_observables_iter->second.fs)) * 1000.0
// << " [ms]!\n";
// // send new tracking command
// const std::shared_ptr<TrackingCmd> trk_cmd_test = std::make_shared<TrackingCmd>(TrackingCmd());
// trk_cmd_test->carrier_freq_hz = 12345.4;
// trk_cmd_test->sample_counter = gnss_observables_iter->second.Tracking_sample_counter;
// trk_cmd_test->channel_id = gnss_observables_iter->second.Channel_ID;
// this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test));
// d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID] = gnss_observables_iter->second.Tracking_sample_counter;
// std::cout << "msg pvt_to_trk sent.\n";
// }
// }
// }
}
// *****************************