diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index defa8377f..927c0aebb 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2126,8 +2126,8 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item try { 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. + // 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. const std::shared_ptr trk_cmd_test = std::make_shared(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++; @@ -2138,50 +2138,50 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item } } d_internal_pvt_solver->vtl_engine.trk_cmd_outs.clear(); - //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"; + // 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"; - // } - // } - // } + // // 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"; + // } + // } + // } } else { @@ -2303,7 +2303,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item { // VTP To.Do: Check why get_PVT is triggered twice. Leave only one get_PVT. - //flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false, false); + // flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false, false); flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0, false, d_enable_vtl, d_close_vtl_loop); } diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 69c1ad204..79628c50d 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -690,8 +690,8 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, double *resp, char *msg, - std::vector &tropo_vec, - std::vector &iono_vec, + std::vector &tropo_vec, + std::vector &iono_vec, std::vector &pr_corrected_code_bias_vec, std::vector &pr_residual_vec, std::vector &doppler_residual_vec) @@ -906,8 +906,8 @@ int raim_fde(const obsd_t *obs, int n, const double *rs, std::vector pr_residual_vec; std::vector doppler_residual_vec; /* estimate receiver position without a satellite */ - if (!estpos(obs_e, n - 1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e, - vsat_e, resp_e, msg_e, iono_vec, tropo_vec, pr_corrected_code_bias_vec,pr_residual_vec,doppler_residual_vec)) + if (!estpos(obs_e, n - 1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e, + vsat_e, resp_e, msg_e, iono_vec, tropo_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec)) { trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg); continue; diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index c6a61372c..0ca0854b9 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -2802,7 +2802,7 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, /* rover position by single point positioning */ if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg, tropo_vec, - iono_vec, pr_corrected_code_bias_vec,pr_residual_vec,doppler_residual_vec)) + iono_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec)) { errmsg(rtk, "point pos error (%s)\n", msg); if (!rtk->opt.dynamics) @@ -2842,9 +2842,9 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, return 1; } if (opt->mode == PMODE_MOVEB) - { /* moving baseline */ + { /* moving baseline */ /* estimate position/velocity of base station */ - if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec, + if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec, iono_vec, pr_corrected_code_bias_vec, pr_residual_vec, doppler_residual_vec)) { errmsg(rtk, "base station position error (%s)\n", msg); diff --git a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc index fcf2b0cdc..8c5bc80bb 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc @@ -602,7 +602,7 @@ void *rtksvrthread(void *arg) std::vector pr_residual; std::vector doppler_residual; rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav, tropo_vec, - iono_vec, pr_corrected_code_bias_vec,pr_residual,doppler_residual); + iono_vec, pr_corrected_code_bias_vec, pr_residual, doppler_residual); rtksvrunlock(svr); if (svr->rtk.sol.stat != SOLQ_NONE) diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 43c09b258..6bfee663b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -657,13 +657,13 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) double old_code_phase_chips = d_x_old_old(0); if (cmd->enable_carrier_nco_cmd) - { - d_x_old_old(2) = tmp_x(2); // replace DOPPLER - } + { + d_x_old_old(2) = tmp_x(2); // replace DOPPLER + } else - { - // std::cout << "correction not applied" << std::endl; - } + { + // std::cout << "correction not applied" << std::endl; + } // set vtl corrections flag to inform VTL from gnss_synchro object d_vtl_cmd_applied_now = true;