mirror of
				https://github.com/gnss-sdr/gnss-sdr
				synced 2025-10-31 15:23:04 +00:00 
			
		
		
		
	Adding complete VTL loop messaging mechanism between PVT and Tracking. Debug prints enabled.
This commit is contained in:
		| @@ -16,7 +16,7 @@ SignalSource.selected_channel=1 | |||||||
| ;# Labsat sile source automatically increments the file name when the signal is split in several files | ;# 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 | ;# 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_ | ;# 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.item_type=gr_complex | ||||||
| SignalSource.sampling_frequency=16368000 | SignalSource.sampling_frequency=16368000 | ||||||
| SignalSource.samples=0 | SignalSource.samples=0 | ||||||
| @@ -99,7 +99,7 @@ Acquisition_1B.dump_filename=./acq_dump.dat | |||||||
| ;######### TRACKING GPS CONFIG ############ | ;######### TRACKING GPS CONFIG ############ | ||||||
| Tracking_1C.implementation=GPS_L1_CA_KF_Tracking | Tracking_1C.implementation=GPS_L1_CA_KF_Tracking | ||||||
| Tracking_1C.item_type=gr_complex | Tracking_1C.item_type=gr_complex | ||||||
| Tracking_1C.dump=true | Tracking_1C.dump=false | ||||||
| Tracking_1C.dump_filename=./tracking_ch_ | Tracking_1C.dump_filename=./tracking_ch_ | ||||||
| Tracking_1C.extend_correlation_symbols=20; | Tracking_1C.extend_correlation_symbols=20; | ||||||
| Tracking_1C.early_late_space_chips=0.5; | Tracking_1C.early_late_space_chips=0.5; | ||||||
|   | |||||||
							
								
								
									
										187
									
								
								conf/gnss-sdr_labsat_kf_vtl.conf
									
									
									
									
									
										Normal file
									
								
							
							
						
						
									
										187
									
								
								conf/gnss-sdr_labsat_kf_vtl.conf
									
									
									
									
									
										Normal file
									
								
							| @@ -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 | ||||||
| @@ -879,6 +879,9 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, | |||||||
|     // Use E6 for PVT |     // 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); |     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 |     // make PVT object | ||||||
|     pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk); |     pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk); | ||||||
|     DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; |     DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; | ||||||
|   | |||||||
| @@ -176,7 +176,9 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, | |||||||
|       d_enable_rx_clock_correction(conf_.enable_rx_clock_correction), |       d_enable_rx_clock_correction(conf_.enable_rx_clock_correction), | ||||||
|       d_an_printer_enabled(conf_.an_output_enabled), |       d_an_printer_enabled(conf_.an_output_enabled), | ||||||
|       d_log_timetag(conf_.log_source_timetag), |       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 |     // Send feedback message to observables block with the receiver clock offset | ||||||
|     this->message_port_register_out(pmt::mp("pvt_to_observables")); |     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; |                     // old_time_debug = d_gnss_observables_map.cbegin()->second.RX_time * 1000.0; | ||||||
|                     uint32_t current_RX_time_ms = 0; |                     uint32_t current_RX_time_ms = 0; | ||||||
|                     // #### solve PVT and store the corrected observable set |                     // #### 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, d_enable_vtl, d_close_vtl_loop)) | ||||||
|                     if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false, get_vtl_data)) |  | ||||||
|                         { |                         { | ||||||
|  |                             // ****** experimental VTL tests | ||||||
|  |                             if (d_close_vtl_loop == true) | ||||||
|  |                                 { | ||||||
|  |                                     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"; | ||||||
|  |                                                         } | ||||||
|  |                                                 } | ||||||
|  |                                         } | ||||||
|  |                                 } | ||||||
|  |                             // ***************************** | ||||||
|  |  | ||||||
|                             d_pvt_errors_counter = 0;  // Reset consecutive PVT error counter |                             d_pvt_errors_counter = 0;  // Reset consecutive PVT error counter | ||||||
|                             const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s(); |                             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 |                     // compute on the fly PVT solution | ||||||
|                     if (flag_compute_pvt_output == true) |                     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) |                     if (flag_pvt_valid == true) | ||||||
|                         { |                         { | ||||||
|                             // experimental VTL tests |  | ||||||
|                             // send 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 = 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 |                             // initialize (if needed) the accumulated phase offset and apply it to the active channels | ||||||
|                             // required to report accumulated phase cycles comparable to pseudoranges |                             // required to report accumulated phase cycles comparable to pseudoranges | ||||||
|                             initialize_and_apply_carrier_phase_offset(); |                             initialize_and_apply_carrier_phase_offset(); | ||||||
|   | |||||||
| @@ -289,6 +289,9 @@ private: | |||||||
|     bool d_an_printer_enabled; |     bool d_an_printer_enabled; | ||||||
|     bool d_log_timetag; |     bool d_log_timetag; | ||||||
|     bool d_use_e6_for_pvt; |     bool d_use_e6_for_pvt; | ||||||
|  |     bool d_enable_vtl; | ||||||
|  |     bool d_close_vtl_loop; | ||||||
|  |     std::map<int, uint64_t> d_last_sent_vtl_cmd_samplestamp_map; | ||||||
| }; | }; | ||||||
|  |  | ||||||
|  |  | ||||||
|   | |||||||
| @@ -92,6 +92,8 @@ public: | |||||||
|     bool dump_mat = true; |     bool dump_mat = true; | ||||||
|     bool log_source_timetag; |     bool log_source_timetag; | ||||||
|     bool use_e6_for_pvt = true; |     bool use_e6_for_pvt = true; | ||||||
|  |     bool enable_vtl = false; | ||||||
|  |     bool close_vtl_loop = true; | ||||||
| }; | }; | ||||||
|  |  | ||||||
|  |  | ||||||
|   | |||||||
| @@ -465,7 +465,7 @@ Monitor_Pvt Rtklib_Solver::get_monitor_pvt() const | |||||||
|     return d_monitor_pvt; |     return d_monitor_pvt; | ||||||
| } | } | ||||||
|  |  | ||||||
| bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, bool flag_averaging, bool get_vtl_data) | bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, bool flag_averaging, bool enable_vtl, bool close_vtl_loop) | ||||||
| { | { | ||||||
|     std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter; |     std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter; | ||||||
|     std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter; |     std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter; | ||||||
| @@ -1065,9 +1065,8 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_ | |||||||
|                         } |                         } | ||||||
|                     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 |                     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) |                     if (enable_vtl == true) | ||||||
|                         { |                         { | ||||||
|                          |  | ||||||
|                             //VTL input data extraction from rtklib structures |                             //VTL input data extraction from rtklib structures | ||||||
|                             /* satellite positions, velocities and clocks */ |                             /* satellite positions, velocities and clocks */ | ||||||
|                             prcopt_t *opt = &d_rtk.opt; |                             prcopt_t *opt = &d_rtk.opt; | ||||||
|   | |||||||
| @@ -86,7 +86,7 @@ public: | |||||||
|         bool use_e6_for_pvt = true); |         bool use_e6_for_pvt = true); | ||||||
|     ~Rtklib_Solver(); |     ~Rtklib_Solver(); | ||||||
|  |  | ||||||
|     bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging, bool get_vtl_data); |     bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging, bool enable_vtl, bool close_vtl_loop); | ||||||
|  |  | ||||||
|     Vtl_Data vtl_data; |     Vtl_Data vtl_data; | ||||||
|  |  | ||||||
|   | |||||||
| @@ -255,6 +255,7 @@ void pcps_acquisition::init() | |||||||
|     d_gnss_synchro->Acq_delay_samples = 0.0; |     d_gnss_synchro->Acq_delay_samples = 0.0; | ||||||
|     d_gnss_synchro->Acq_doppler_hz = 0.0; |     d_gnss_synchro->Acq_doppler_hz = 0.0; | ||||||
|     d_gnss_synchro->Acq_samplestamp_samples = 0ULL; |     d_gnss_synchro->Acq_samplestamp_samples = 0ULL; | ||||||
|  |     d_gnss_synchro->last_vtl_cmd_sample_counter = 0ULL; | ||||||
|     d_mag = 0.0; |     d_mag = 0.0; | ||||||
|     d_input_power = 0.0; |     d_input_power = 0.0; | ||||||
|  |  | ||||||
|   | |||||||
| @@ -34,6 +34,7 @@ public: | |||||||
|     double carrier_freq_hz = 0.0; |     double carrier_freq_hz = 0.0; | ||||||
|     double carrier_freq_rate_hz_s = 0.0; |     double carrier_freq_rate_hz_s = 0.0; | ||||||
|     uint64_t sample_counter = 0UL; |     uint64_t sample_counter = 0UL; | ||||||
|  |     uint32_t channel_id = 0; | ||||||
| }; | }; | ||||||
|  |  | ||||||
| /** \} */ | /** \} */ | ||||||
|   | |||||||
| @@ -129,7 +129,9 @@ kf_tracking::kf_tracking(const Kf_Conf &conf_) | |||||||
|       d_cloop(true), |       d_cloop(true), | ||||||
|       d_dump(d_trk_parameters.dump), |       d_dump(d_trk_parameters.dump), | ||||||
|       d_dump_mat(d_trk_parameters.dump_mat && d_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 |     // prevent telemetry symbols accumulation in output buffers | ||||||
|     this->set_max_noutput_items(1); |     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<TrackingCmd>).hash_code()) |             if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr<TrackingCmd>).hash_code()) | ||||||
|                 { |                 { | ||||||
|                     const auto cmd = wht::any_cast<const std::shared_ptr<TrackingCmd>>(pmt::any_ref(msg)); |                     const auto cmd = wht::any_cast<const std::shared_ptr<TrackingCmd>>(pmt::any_ref(msg)); | ||||||
|                     // std::cout << "RX pvt-to-trk cmd with delay: " |                     if (cmd->channel_id == this->d_channel) | ||||||
|                     //           << static_cast<double>(nitems_read(0) - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n"; |                         { | ||||||
|  |                             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<double>(d_sample_counter - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n"; | ||||||
|  |                         } | ||||||
|                 } |                 } | ||||||
|             else |             else | ||||||
|                 { |                 { | ||||||
| @@ -2071,6 +2088,11 @@ int kf_tracking::general_work(int noutput_items __attribute__((unused)), gr_vect | |||||||
|         { |         { | ||||||
|             current_synchro_data.fs = static_cast<int64_t>(d_trk_parameters.fs_in); |             current_synchro_data.fs = static_cast<int64_t>(d_trk_parameters.fs_in); | ||||||
|             current_synchro_data.Tracking_sample_counter = d_sample_counter; |             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; |             *out[0] = current_synchro_data; | ||||||
|             return 1; |             return 1; | ||||||
|         } |         } | ||||||
|   | |||||||
| @@ -230,6 +230,9 @@ private: | |||||||
|     bool d_dump; |     bool d_dump; | ||||||
|     bool d_dump_mat; |     bool d_dump_mat; | ||||||
|     bool d_acc_carrier_phase_initialized; |     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; |     bool d_enable_extended_integration; | ||||||
| }; | }; | ||||||
|  |  | ||||||
|   | |||||||
| @@ -80,6 +80,9 @@ public: | |||||||
|     bool Flag_valid_pseudorange{};         //!< Set by Observables processing block |     bool Flag_valid_pseudorange{};         //!< Set by Observables processing block | ||||||
|     bool Flag_PLL_180_deg_phase_locked{};  //!< Set by Telemetry Decoder 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 |     /// Copy constructor | ||||||
|     Gnss_Synchro(const Gnss_Synchro& other) noexcept |     Gnss_Synchro(const Gnss_Synchro& other) noexcept | ||||||
|     { |     { | ||||||
| @@ -120,6 +123,7 @@ public: | |||||||
|                 this->Flag_valid_word = rhs.Flag_valid_word; |                 this->Flag_valid_word = rhs.Flag_valid_word; | ||||||
|                 this->Flag_valid_pseudorange = rhs.Flag_valid_pseudorange; |                 this->Flag_valid_pseudorange = rhs.Flag_valid_pseudorange; | ||||||
|                 this->Flag_PLL_180_deg_phase_locked = rhs.Flag_PLL_180_deg_phase_locked; |                 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; |         return *this; | ||||||
|     }; |     }; | ||||||
| @@ -163,6 +167,7 @@ public: | |||||||
|                 this->Flag_valid_word = other.Flag_valid_word; |                 this->Flag_valid_word = other.Flag_valid_word; | ||||||
|                 this->Flag_valid_pseudorange = other.Flag_valid_pseudorange; |                 this->Flag_valid_pseudorange = other.Flag_valid_pseudorange; | ||||||
|                 this->Flag_PLL_180_deg_phase_locked = other.Flag_PLL_180_deg_phase_locked; |                 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; |         return *this; | ||||||
|     }; |     }; | ||||||
|   | |||||||
		Reference in New Issue
	
	Block a user
	 Javier Arribas
					Javier Arribas