mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-31 07:13:03 +00:00
Code Issues Releases Wiki Activity

Adding complete VTL loop messaging mechanism between PVT and Tracking. Debug prints enabled.

Browse Source
This commit is contained in:
Javier Arribas
2022-10-13 13:33:10 +02:00
parent 6fad047070
commit 73098ffe34
13 changed files with 354 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
conf/gnss-sdr_labsat_kf.conf
View File

@@ -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;

187
conf/gnss-sdr_labsat_kf_vtl.conf Normal file
View 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

3
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@@ -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() << ")";

67
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@@ -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<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
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<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
// required to report accumulated phase cycles comparable to pseudoranges
initialize_and_apply_carrier_phase_offset();

3
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h
View File

@@ -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<int, uint64_t> d_last_sent_vtl_cmd_samplestamp_map;
};

2
src/algorithms/PVT/libs/pvt_conf.h
View File

@@ -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;
};

5
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@@ -465,7 +465,7 @@ Monitor_Pvt Rtklib_Solver::get_monitor_pvt() const
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, 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
if (get_vtl_data == true)
if (enable_vtl == true)
{
//VTL input data extraction from rtklib structures
/* satellite positions, velocities and clocks */
prcopt_t *opt = &d_rtk.opt;

2
src/algorithms/PVT/libs/rtklib_solver.h
View File

@@ -86,7 +86,7 @@ public:
bool use_e6_for_pvt = true);
~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;

1
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc
View File

@@ -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;

1
src/algorithms/libs/trackingcmd.h
View File

@@ -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;
};
/** \} */

28
src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
View File

@@ -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<TrackingCmd>).hash_code())
{
const auto cmd = wht::any_cast<const std::shared_ptr<TrackingCmd>>(pmt::any_ref(msg));
// std::cout << "RX pvt-to-trk cmd with delay: "
// << static_cast<double>(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<double>(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<int64_t>(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;
}

3
src/algorithms/tracking/gnuradio_blocks/kf_tracking.h
View File

@@ -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;
};

5
src/core/system_parameters/gnss_synchro.h
View File

@@ -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;
};