diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 2bce8f9b3..fb60ade51 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -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::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 trk_cmd_test = std::make_shared(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(gnss_observables_iter->second.Tracking_sample_counter) / static_cast(gnss_observables_iter->second.fs) - // << " T_last_vtl_trk: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) / static_cast(gnss_observables_iter->second.fs) - // << " T_map: " << static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) / static_cast(gnss_observables_iter->second.fs) - // << " T2: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) - // << " T3: " << static_cast(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(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(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])) / static_cast(gnss_observables_iter->second.fs)) * 1000.0 - << " [ms]!\n"; - // send new tracking command - const std::shared_ptr trk_cmd_test = std::make_shared(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::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 trk_cmd_test = std::make_shared(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(gnss_observables_iter->second.Tracking_sample_counter) / static_cast(gnss_observables_iter->second.fs) + // // << " T_last_vtl_trk: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) / static_cast(gnss_observables_iter->second.fs) + // // << " T_map: " << static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) / static_cast(gnss_observables_iter->second.fs) + // // << " T2: " << static_cast(gnss_observables_iter->second.last_vtl_cmd_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID]) + // // << " T3: " << static_cast(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(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(gnss_observables_iter->second.Tracking_sample_counter) - static_cast(d_last_sent_vtl_cmd_samplestamp_map[gnss_observables_iter->second.Channel_ID])) / static_cast(gnss_observables_iter->second.fs)) * 1000.0 + // << " [ms]!\n"; + // // send new tracking command + // const std::shared_ptr trk_cmd_test = std::make_shared(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"; + // } + // } + // } } // *****************************