mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-15 06:14:58 +00:00
Adding complete VTL loop messaging mechanism between PVT and Tracking. Debug prints enabled.
This commit is contained in:
Javier Arribas
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
|
||||
;# 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
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
|
||||
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() << ")";
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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;
|
||||
@ -1064,87 +1064,86 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &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<int> 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<int> 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<double, 3> pos{};
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
/** \} */
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user