From 73098ffe34f57d2debcb65a54449397c40deb75f Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 13 Oct 2022 13:33:10 +0200 Subject: [PATCH] Adding complete VTL loop messaging mechanism between PVT and Tracking. Debug prints enabled. --- conf/gnss-sdr_labsat_kf.conf | 4 +- conf/gnss-sdr_labsat_kf_vtl.conf | 187 ++++++++++++++++++ src/algorithms/PVT/adapters/rtklib_pvt.cc | 3 + .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 67 +++++-- .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 3 + src/algorithms/PVT/libs/pvt_conf.h | 2 + src/algorithms/PVT/libs/rtklib_solver.cc | 131 ++++++------ src/algorithms/PVT/libs/rtklib_solver.h | 2 +- .../gnuradio_blocks/pcps_acquisition.cc | 1 + src/algorithms/libs/trackingcmd.h | 1 + .../tracking/gnuradio_blocks/kf_tracking.cc | 28 ++- .../tracking/gnuradio_blocks/kf_tracking.h | 3 + src/core/system_parameters/gnss_synchro.h | 5 + 13 files changed, 354 insertions(+), 83 deletions(-) create mode 100644 conf/gnss-sdr_labsat_kf_vtl.conf diff --git a/conf/gnss-sdr_labsat_kf.conf b/conf/gnss-sdr_labsat_kf.conf index 31787abab..15d77743b 100644 --- a/conf/gnss-sdr_labsat_kf.conf +++ b/conf/gnss-sdr_labsat_kf.conf @@ -16,7 +16,7 @@ SignalSource.selected_channel=1 ;# Labsat sile source automatically increments the file name when the signal is split in several files ;# the adapter adds "_0000.LS3" to this base path and filename. Next file will be "_0001.LS3" and so on ;# in this example, the first file complete path will be ../signals/GPS_025_ -SignalSource.filename=/home/javier/signals/satgen_30mins/output/output +SignalSource.filename=/home/javier/signals/satgen_30m/output/output SignalSource.item_type=gr_complex SignalSource.sampling_frequency=16368000 SignalSource.samples=0 @@ -99,7 +99,7 @@ Acquisition_1B.dump_filename=./acq_dump.dat ;######### TRACKING GPS CONFIG ############ Tracking_1C.implementation=GPS_L1_CA_KF_Tracking Tracking_1C.item_type=gr_complex -Tracking_1C.dump=true +Tracking_1C.dump=false Tracking_1C.dump_filename=./tracking_ch_ Tracking_1C.extend_correlation_symbols=20; Tracking_1C.early_late_space_chips=0.5; diff --git a/conf/gnss-sdr_labsat_kf_vtl.conf b/conf/gnss-sdr_labsat_kf_vtl.conf new file mode 100644 index 000000000..976df11d8 --- /dev/null +++ b/conf/gnss-sdr_labsat_kf_vtl.conf @@ -0,0 +1,187 @@ +; This is a GNSS-SDR configuration file +; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/ +; SPDX-License-Identifier: GPL-3.0-or-later +; SPDX-FileCopyrightText: (C) 2010-2021 (see AUTHORS file for a list of contributors) + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +GNSS-SDR.internal_fs_sps=5456000 +GNSS-SDR.use_acquisition_resampler=true + +;######### SIGNAL_SOURCE CONFIG ############ +SignalSource.implementation=Labsat_Signal_Source +SignalSource.selected_channel=1 +;#filename: path to file with the captured GNSS signal samples to be processed +;# Labsat sile source automatically increments the file name when the signal is split in several files +;# the adapter adds "_0000.LS3" to this base path and filename. Next file will be "_0001.LS3" and so on +;# in this example, the first file complete path will be ../signals/GPS_025_ +SignalSource.filename=/home/javier/signals/satgen_30m/output/output +SignalSource.item_type=gr_complex +SignalSource.sampling_frequency=16368000 +SignalSource.samples=0 +SignalSource.repeat=false +SignalSource.dump=false +SignalSource.dump_filename=./out.dat +SignalSource.enable_throttle_control=false + + +;######### SIGNAL_CONDITIONER CONFIG ############ +SignalConditioner.implementation=Signal_Conditioner + +;######### DATA_TYPE_ADAPTER CONFIG ############ +DataTypeAdapter.implementation=Pass_Through +DataTypeAdapter.item_type=gr_complex + +;######### INPUT_FILTER CONFIG ############ +InputFilter.implementation=Freq_Xlating_Fir_Filter +InputFilter.dump=false +InputFilter.dump_filename=/media/javier/WDNASNTFS/output_5.456Msps_gr_complex.dat + +InputFilter.input_item_type=gr_complex +InputFilter.output_item_type=gr_complex +InputFilter.taps_item_type=float +InputFilter.number_of_taps=5 +InputFilter.number_of_bands=2 + +InputFilter.band1_begin=0.0 +InputFilter.band1_end=0.45 +InputFilter.band2_begin=0.55 +InputFilter.band2_end=1.0 + +InputFilter.ampl1_begin=1.0 +InputFilter.ampl1_end=1.0 +InputFilter.ampl2_begin=0.0 +InputFilter.ampl2_end=0.0 + +InputFilter.band1_error=1.0 +InputFilter.band2_error=1.0 + +InputFilter.filter_type=lowpass +InputFilter.grid_density=16 +InputFilter.sampling_frequency=16368000 +InputFilter.IF=0 +InputFilter.decimation_factor=3 + + +;######### CHANNELS GLOBAL CONFIG ############ +Channels_1C.count=6 +Channels_1B.count=0 +Channels_L5.count=0 +Channels_5X.count=0 + +Channels.in_acquisition=1 + +;######### GPS ACQUISITION CONFIG ############ +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.item_type=gr_complex +Acquisition_1C.threshold=3.0 +Acquisition_1C.use_CFAR_algorithm=false +Acquisition_1C.blocking=true +Acquisition_1C.doppler_max=5000 +Acquisition_1C.doppler_step=125 +Acquisition_1C.dump=false +Acquisition_1C.dump_filename=./acq_dump.dat + + +;######### GALILEO ACQUISITION CONFIG ############ +Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition +Acquisition_1B.item_type=gr_complex +Acquisition_1B.threshold=2.8 +Acquisition_1B.use_CFAR_algorithm=false +Acquisition_1B.blocking=false +Acquisition_1B.doppler_max=5000 +Acquisition_1B.doppler_step=125 +Acquisition_1B.dump=false +Acquisition_1B.dump_filename=./acq_dump.dat + + +;######### TRACKING GPS CONFIG ############ +Tracking_1C.implementation=GPS_L1_CA_KF_Tracking +Tracking_1C.item_type=gr_complex +Tracking_1C.dump=false +Tracking_1C.dump_filename=./tracking_ch_ +Tracking_1C.extend_correlation_symbols=1; +Tracking_1C.early_late_space_chips=0.5; +Tracking_1C.early_late_space_narrow_chips=0.15 + +;Tracking_1C.code_disc_sd_chips=0.2; // Initial R +;Tracking_1C.carrier_disc_sd_rads=0.3; // Initial R + +;Tracking_1C.init_code_phase_sd_chips=0.5; // Initial P_0_0 +;Tracking_1C.init_carrier_phase_sd_rad=0.7; +;Tracking_1C.init_carrier_freq_sd_hz=5; +;Tracking_1C.init_carrier_freq_rate_sd_hz_s=1; + +;Tracking_1C.code_phase_sd_chips=0.15; // Initial Q +;Tracking_1C.carrier_phase_sd_rad=0.25; +;Tracking_1C.carrier_freq_sd_hz=0.6; +;Tracking_1C.carrier_freq_rate_sd_hz_s=0.01; + + +;######### TRACKING GALILEO CONFIG ############ +Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking +Tracking_1B.item_type=gr_complex +Tracking_1B.pll_bw_hz=15.0; +Tracking_1B.dll_bw_hz=0.75; +Tracking_1B.early_late_space_chips=0.15; +Tracking_1B.very_early_late_space_chips=0.5; +Tracking_1B.early_late_space_narrow_chips=0.10; +Tracking_1B.very_early_late_space_narrow_chips=0.5; +Tracking_1B.pll_bw_narrow_hz=2.5 +Tracking_1B.dll_bw_narrow_hz=0.2 +Tracking_1B.extend_correlation_symbols=5 +Tracking_1B.track_pilot=true +Tracking_1B.enable_fll_pull_in=true; +;Tracking_1B.pull_in_time_s=60 +Tracking_1B.enable_fll_steady_state=false +Tracking_1B.fll_bw_hz=10 +Tracking_1B.dump=false +Tracking_1B.dump_filename=tracking_ch_ + +;######### TELEMETRY DECODER GALILEO CONFIG ############ +TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder +TelemetryDecoder_1B.dump=false + + +;######### TELEMETRY DECODER GPS CONFIG ############ +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1C.dump=false + + +;######### OBSERVABLES CONFIG ############ +;#implementation: +Observables.implementation=Hybrid_Observables +Observables.dump=false +Observables.dump_filename=./observables.dat +Observables.enable_carrier_smoothing=false +Observables.smoothing_factor=200 + + + +;######### PVT CONFIG ############ +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=Single ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.enable_rx_clock_correction=false +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad +PVT.output_rate_ms=100; +PVT.rinexobs_rate_ms=1000; +PVT.display_rate_ms=1000; +PVT.elevation_mask=15; +PVT.flag_rtcm_server=false +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 +PVT.dump=false +PVT.dump_filename=./PVT +PVT.enable_monitor=false +PVT.monitor_udp_port=1337 +PVT.monitor_client_addresses=127.0.0.1 +PVT.enable_vtl=false +PVT.close_vtl_loop=true + +;######### MONITOR CONFIG ############ +Monitor.enable_monitor=false +Monitor.decimation_factor=1 +Monitor.client_addresses=127.0.0.1 +Monitor.udp_port=1234 diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index ddadd7242..abb17567f 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -879,6 +879,9 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, // Use E6 for PVT pvt_output_parameters.use_e6_for_pvt = configuration->property(role + ".use_e6_for_pvt", pvt_output_parameters.use_e6_for_pvt); + // Vector Tracking Loop (VTL) + pvt_output_parameters.enable_vtl = configuration->property(role + ".enable_vtl", pvt_output_parameters.enable_vtl); + pvt_output_parameters.close_vtl_loop = configuration->property(role + ".close_vtl_loop", pvt_output_parameters.close_vtl_loop); // make PVT object pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk); DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index bb07f6558..e34bf8fa1 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -176,7 +176,9 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_enable_rx_clock_correction(conf_.enable_rx_clock_correction), d_an_printer_enabled(conf_.an_output_enabled), d_log_timetag(conf_.log_source_timetag), - d_use_e6_for_pvt(conf_.use_e6_for_pvt) + d_use_e6_for_pvt(conf_.use_e6_for_pvt), + d_enable_vtl(conf_.enable_vtl), + d_close_vtl_loop(conf_.close_vtl_loop) { // Send feedback message to observables block with the receiver clock offset this->message_port_register_out(pmt::mp("pvt_to_observables")); @@ -2131,9 +2133,57 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // old_time_debug = d_gnss_observables_map.cbegin()->second.RX_time * 1000.0; uint32_t current_RX_time_ms = 0; // #### solve PVT and store the corrected observable set - bool get_vtl_data = true; - if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false, get_vtl_data)) + 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) + { + 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"; + } + } + } + } + // ***************************** + d_pvt_errors_counter = 0; // Reset consecutive PVT error counter const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s(); @@ -2246,18 +2296,13 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // compute on the fly PVT solution if (flag_compute_pvt_output == true) { - flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false); + // 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); } if (flag_pvt_valid == true) { - // experimental VTL tests - // send 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 = d_gnss_observables_map.begin()->second.Tracking_sample_counter; - // this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); - // initialize (if needed) the accumulated phase offset and apply it to the active channels // required to report accumulated phase cycles comparable to pseudoranges initialize_and_apply_carrier_phase_offset(); diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index cb8a2053b..3f803260b 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -289,6 +289,9 @@ private: bool d_an_printer_enabled; bool d_log_timetag; bool d_use_e6_for_pvt; + bool d_enable_vtl; + bool d_close_vtl_loop; + std::map d_last_sent_vtl_cmd_samplestamp_map; }; diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index 34852c9c8..1924cc9eb 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -92,6 +92,8 @@ public: bool dump_mat = true; bool log_source_timetag; bool use_e6_for_pvt = true; + bool enable_vtl = false; + bool close_vtl_loop = true; }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 816851ca3..ec584acce 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -465,7 +465,7 @@ Monitor_Pvt Rtklib_Solver::get_monitor_pvt() const return d_monitor_pvt; } -bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging, bool get_vtl_data) +bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging, bool enable_vtl, bool close_vtl_loop) { std::map::const_iterator gnss_observables_iter; std::map::const_iterator galileo_ephemeris_iter; @@ -1064,87 +1064,86 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ rx_position_and_time[3] = pvt_sol.dtr[2] + pvt_sol.dtr[0] / SPEED_OF_LIGHT_M_S; } this->set_rx_pos({rx_position_and_time[0], rx_position_and_time[1], rx_position_and_time[2]}); // save ECEF position for the next iteration - - if (get_vtl_data == true) - { - - //VTL input data extraction from rtklib structures - /* satellite positions, velocities and clocks */ - prcopt_t *opt = &d_rtk.opt; - /* count rover/base station observations */ - int n_sats = valid_obs + glo_valid_obs; - int nu; - int nr; - for (nu = 0; nu < n_sats && d_obs_data.data()[nu].rcv == 1; nu++) - { - } - for (nr = 0; nu + nr < n_sats && d_obs_data.data()[nu + nr].rcv == 2; nr++) - { - } - double *rs; - double *dts; - double *var; + if (enable_vtl == true) + { + //VTL input data extraction from rtklib structures + /* satellite positions, velocities and clocks */ + prcopt_t *opt = &d_rtk.opt; + /* count rover/base station observations */ + int n_sats = valid_obs + glo_valid_obs; + int nu; + int nr; + for (nu = 0; nu < n_sats && d_obs_data.data()[nu].rcv == 1; nu++) + { + } + for (nr = 0; nu + nr < n_sats && d_obs_data.data()[nu + nr].rcv == 2; nr++) + { + } - std::vector svh(MAXOBS * 2); - rs = mat(6, n_sats); - dts = mat(2, n_sats); - var = mat(1, n_sats); - /* satellite positions, velocities and clocks */ - satposs(d_rtk.sol.time, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, opt->sateph, rs, dts, var, svh.data()); + double *rs; + double *dts; + double *var; - Vtl_Data new_vtl_data; - new_vtl_data.init_storage(n_sats); - new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; - new_vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands - new_vtl_data.sat_number=n_sats; - for (int n = 0; n < n_sats; n++) - { - new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; - new_vtl_data.sat_p(n, 1) = rs[1 + 6 * n]; - new_vtl_data.sat_p(n, 2) = rs[2 + 6 * n]; - new_vtl_data.sat_v(n, 0) = rs[3 + 6 * n]; - new_vtl_data.sat_v(n, 1) = rs[4 + 6 * n]; - new_vtl_data.sat_v(n, 2) = rs[5 + 6 * n]; - - new_vtl_data.sat_dts(n, 0) = dts[0 + 2 * n]; - new_vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; - new_vtl_data.sat_var(n) = var[n]; - new_vtl_data.sat_health_flag(n) = svh.at(n); - new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; - // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) - new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; - new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; - new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; - } + std::vector svh(MAXOBS * 2); + rs = mat(6, n_sats); + dts = mat(2, n_sats); + var = mat(1, n_sats); + /* satellite positions, velocities and clocks */ + satposs(d_rtk.sol.time, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, opt->sateph, rs, dts, var, svh.data()); + + Vtl_Data new_vtl_data; + new_vtl_data.init_storage(n_sats); + new_vtl_data.epoch_tow_s = gnss_observables_map.cbegin()->second.RX_time; + new_vtl_data.sample_counter = gnss_observables_map.cbegin()->second.Tracking_sample_counter; // TODO: check if the different tracking instants (different sample_counters) affect the VTL commands + new_vtl_data.sat_number = n_sats; + for (int n = 0; n < n_sats; n++) + { + new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; + new_vtl_data.sat_p(n, 1) = rs[1 + 6 * n]; + new_vtl_data.sat_p(n, 2) = rs[2 + 6 * n]; + new_vtl_data.sat_v(n, 0) = rs[3 + 6 * n]; + new_vtl_data.sat_v(n, 1) = rs[4 + 6 * n]; + new_vtl_data.sat_v(n, 2) = rs[5 + 6 * n]; + + new_vtl_data.sat_dts(n, 0) = dts[0 + 2 * n]; + new_vtl_data.sat_dts(n, 1) = dts[1 + 2 * n]; + new_vtl_data.sat_var(n) = var[n]; + new_vtl_data.sat_health_flag(n) = svh.at(n); + new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; + // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) + new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; + new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; + new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; + } //VTL input data extraction from rtklib structures /* Receiver position, velocity and clock */ /* position/velocity (m|m/s):{x,y,z,vx,vy,vz} or {e,n,u,ve,vn,vu} */ - new_vtl_data.rx_p(0) =pvt_sol.rr[0]; - new_vtl_data.rx_p(1) =pvt_sol.rr[1]; - new_vtl_data.rx_p(2) =pvt_sol.rr[2]; - new_vtl_data.rx_v(0) =pvt_sol.rr[3] ; - new_vtl_data.rx_v(1) =pvt_sol.rr[4] ; - new_vtl_data.rx_v(2) =pvt_sol.rr[5] ; + new_vtl_data.rx_p(0) = pvt_sol.rr[0]; + new_vtl_data.rx_p(1) = pvt_sol.rr[1]; + new_vtl_data.rx_p(2) = pvt_sol.rr[2]; + new_vtl_data.rx_v(0) = pvt_sol.rr[3]; + new_vtl_data.rx_v(1) = pvt_sol.rr[4]; + new_vtl_data.rx_v(2) = pvt_sol.rr[5]; /* Receiver position, velocity and clock variances*/ new_vtl_data.rx_pvt_var[0] = pvt_sol.qr[0]; new_vtl_data.rx_pvt_var[1] = pvt_sol.qr[1]; new_vtl_data.rx_pvt_var[2] = pvt_sol.qr[2]; //TODO: get direct estimations for V T variances, instead: - new_vtl_data.rx_pvt_var[3] = pvt_sol.qr[0]*0.1; //in general minor than position. - new_vtl_data.rx_pvt_var[4] = pvt_sol.qr[1]*0.1; - new_vtl_data.rx_pvt_var[5] = pvt_sol.qr[2]*0.1; - new_vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time - new_vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler + new_vtl_data.rx_pvt_var[3] = pvt_sol.qr[0] * 0.1; //in general minor than position. + new_vtl_data.rx_pvt_var[4] = pvt_sol.qr[1] * 0.1; + new_vtl_data.rx_pvt_var[5] = pvt_sol.qr[2] * 0.1; + new_vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time + new_vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler //receiver clock offset and receiver clock drift - new_vtl_data.rx_dts(0)=rx_position_and_time[3]; - new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]; - + new_vtl_data.rx_dts(0) = rx_position_and_time[3]; + new_vtl_data.rx_dts(1) = pvt_sol.dtr[5]; + //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? vtl_engine.vtl_loop(new_vtl_data); new_vtl_data.debug_print(); - } + } // compute Ground speed and COG double ground_speed_ms = 0.0; std::array pos{}; diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index b03cc886d..0c8b3dd26 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -86,7 +86,7 @@ public: bool use_e6_for_pvt = true); ~Rtklib_Solver(); - bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging, bool get_vtl_data); + bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging, bool enable_vtl, bool close_vtl_loop); Vtl_Data vtl_data; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index a8ac0428e..4148a71c5 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -255,6 +255,7 @@ void pcps_acquisition::init() d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; d_gnss_synchro->Acq_samplestamp_samples = 0ULL; + d_gnss_synchro->last_vtl_cmd_sample_counter = 0ULL; d_mag = 0.0; d_input_power = 0.0; diff --git a/src/algorithms/libs/trackingcmd.h b/src/algorithms/libs/trackingcmd.h index 77115167b..7e51d5e97 100644 --- a/src/algorithms/libs/trackingcmd.h +++ b/src/algorithms/libs/trackingcmd.h @@ -34,6 +34,7 @@ public: double carrier_freq_hz = 0.0; double carrier_freq_rate_hz_s = 0.0; uint64_t sample_counter = 0UL; + uint32_t channel_id = 0; }; /** \} */ diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 53856bb4a..66ad1e51c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -129,7 +129,9 @@ kf_tracking::kf_tracking(const Kf_Conf &conf_) d_cloop(true), d_dump(d_trk_parameters.dump), d_dump_mat(d_trk_parameters.dump_mat && d_dump), - d_acc_carrier_phase_initialized(false) + d_acc_carrier_phase_initialized(false), + d_vtl_cmd_applied_now(false), + d_vtl_cmd_samplestamp(0LL) { // prevent telemetry symbols accumulation in output buffers this->set_max_noutput_items(1); @@ -626,8 +628,23 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr).hash_code()) { const auto cmd = wht::any_cast>(pmt::any_ref(msg)); - // std::cout << "RX pvt-to-trk cmd with delay: " - // << static_cast(nitems_read(0) - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n"; + if (cmd->channel_id == this->d_channel) + { + gr::thread::scoped_lock lock(d_setlock); + //To.Do: apply VTL corrections to the KF states + // code + // d_code_error_kf_chips; + // d_code_freq_kf_chips_s; + // // carrier + // d_carrier_phase_kf_rad; + // d_carrier_doppler_kf_hz; + // d_carrier_doppler_rate_kf_hz_s; + // 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(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n"; + } } else { @@ -2071,6 +2088,11 @@ int kf_tracking::general_work(int noutput_items __attribute__((unused)), gr_vect { current_synchro_data.fs = static_cast(d_trk_parameters.fs_in); current_synchro_data.Tracking_sample_counter = d_sample_counter; + if (d_vtl_cmd_applied_now == true) + { + d_vtl_cmd_applied_now = false; + } + current_synchro_data.last_vtl_cmd_sample_counter = d_vtl_cmd_samplestamp; *out[0] = current_synchro_data; return 1; } diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.h b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.h index 37e38b679..bbdfc51cd 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.h @@ -230,6 +230,9 @@ private: bool d_dump; bool d_dump_mat; bool d_acc_carrier_phase_initialized; + // VTL cmd control + bool d_vtl_cmd_applied_now; + uint64_t d_vtl_cmd_samplestamp; bool d_enable_extended_integration; }; diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index f2934f5de..a8cab3ec8 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -80,6 +80,9 @@ public: bool Flag_valid_pseudorange{}; //!< Set by Observables processing block bool Flag_PLL_180_deg_phase_locked{}; //!< Set by Telemetry Decoder processing block + // VTL + uint64_t last_vtl_cmd_sample_counter{}; //!< Set by Tracking processing block + /// Copy constructor Gnss_Synchro(const Gnss_Synchro& other) noexcept { @@ -120,6 +123,7 @@ public: this->Flag_valid_word = rhs.Flag_valid_word; this->Flag_valid_pseudorange = rhs.Flag_valid_pseudorange; this->Flag_PLL_180_deg_phase_locked = rhs.Flag_PLL_180_deg_phase_locked; + this->last_vtl_cmd_sample_counter = rhs.last_vtl_cmd_sample_counter; } return *this; }; @@ -163,6 +167,7 @@ public: this->Flag_valid_word = other.Flag_valid_word; this->Flag_valid_pseudorange = other.Flag_valid_pseudorange; this->Flag_PLL_180_deg_phase_locked = other.Flag_PLL_180_deg_phase_locked; + this->last_vtl_cmd_sample_counter = other.last_vtl_cmd_sample_counter; } return *this; };