mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-06-16 21:44:14 +00:00
Adding complete VTL loop messaging mechanism between PVT and Tracking. Debug prints enabled.
This commit is contained in:
parent
6fad047070
commit
73098ffe34
@ -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;
|
||||||
};
|
};
|
||||||
|
Loading…
x
Reference in New Issue
Block a user