diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index bdd2f21e5..c3716e594 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -90,6 +90,32 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, SensorDataSourceConfiguration sensor_data_configuration{configuration}; pvt_output_parameters.kf_use_imu_vel = configuration->property(role + ".kf_use_imu_vel", false); + // PVT VTL settings + const int gps_1C_count = configuration->property("Channels_1C.count", 0); + const int gps_2S_count = configuration->property("Channels_2S.count", 0); + const int gps_L5_count = configuration->property("Channels_L5.count", 0); + const int gal_1B_count = configuration->property("Channels_1B.count", 0); + const int gal_E5a_count = configuration->property("Channels_5X.count", 0); + const int gal_E5b_count = configuration->property("Channels_7X.count", 0); + const int gal_E6_count = configuration->property("Channels_E6.count", 0); + pvt_output_parameters.enable_pvt_vtl = configuration->property(role + ".enable_pvt_vtl", false); + pvt_output_parameters.enable_pvt_output_vtl = configuration->property(role + ".enable_pvt_output_vtl", false); + pvt_output_parameters.enable_pvt_closure_vtl = configuration->property(role + ".enable_pvt_closure_vtl", false); + pvt_output_parameters.vtl_kinematic = configuration->property(role + ".vtl_kinematic", false); + pvt_output_parameters.vtl_dump = configuration->property(role + ".vtl_dump", pvt_output_parameters.vtl_dump); + pvt_output_parameters.vtl_dump_filename = configuration->property(role + ".vtl_dump_filename", pvt_output_parameters.vtl_dump_filename); + pvt_output_parameters.vtl_gps_channels = gps_1C_count + gps_2S_count + gps_L5_count; + pvt_output_parameters.vtl_gal_channels = gal_1B_count + gal_E5a_count + gal_E5b_count + gal_E6_count; + pvt_output_parameters.vtl_init_pos_ecef_sd_m = configuration->property(role + ".vtl_init_pos_ecef_sd_m", 10.0); + pvt_output_parameters.vtl_init_vel_ecef_sd_ms = configuration->property(role + ".vtl_init_vel_ecef_sd_ms", 5.0); + pvt_output_parameters.vtl_init_clk_b_sd_m = configuration->property(role + ".vtl_init_clk_b_sd_m", 100.0); + pvt_output_parameters.vtl_init_clk_d_sd_ms = configuration->property(role + ".vtl_init_clk_d_sd_ms", 100.0); + pvt_output_parameters.vtl_sys_acc_noise_sd_ms2 = configuration->property(role + ".vtl_sys_acc_noise_sd_ms2", 0.1); + pvt_output_parameters.vtl_sys_clk_b_noise_sd_m = configuration->property(role + ".vtl_sys_clk_b_noise_sd_m", 0); + pvt_output_parameters.vtl_sys_clk_d_noise_sd_ms = configuration->property(role + ".vtl_sys_clk_d_noise_sd_ms", 0); + pvt_output_parameters.vtl_meas_prange_sd_m = configuration->property(role + ".vtl_meas_prange_sd_m", 7.75); + pvt_output_parameters.vtl_meas_prange_rate_sd_ms = configuration->property(role + ".vtl_meas_prange_rate_sd_ms", 0.45); + // NMEA Printer settings pvt_output_parameters.flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false); pvt_output_parameters.nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename); diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt old mode 100644 new mode 100755 index ae65cab84..aac9ce12f --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -25,6 +25,8 @@ set(PVT_LIB_SOURCES geohash.cc pvt_kf.cc signal_enabled_flags.cc + receiver_type.cc + vtl_data.cc ) set(PVT_LIB_HEADERS diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index a9da84313..f13a48222 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -106,6 +106,25 @@ public: // Sensor Data parameters bool kf_use_imu_vel = false; + + // PVT VTL parameters + bool enable_pvt_vtl = false; + bool enable_pvt_output_vtl = false; + bool enable_pvt_closure_vtl = false; + bool vtl_kinematic = false; + bool vtl_dump = false; + std::string vtl_dump_filename; + int vtl_gps_channels = 5; + int vtl_gal_channels = 5; + double vtl_init_pos_ecef_sd_m = 10.0; + double vtl_init_vel_ecef_sd_ms = 5.0; + double vtl_init_clk_b_sd_m = 100.0; + double vtl_init_clk_d_sd_ms = 100.0; + double vtl_sys_acc_noise_sd_ms2 = 0.1; + double vtl_sys_clk_b_noise_sd_m = 0.0; + double vtl_sys_clk_d_noise_sd_ms = 0.0; + double vtl_meas_prange_sd_m = 7.75; + double vtl_meas_prange_rate_sd_ms = 0.45; }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc old mode 100644 new mode 100755 index c61df11d5..fef2dfcb6 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -57,7 +57,8 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, d_conf(conf), d_signal_enabled_flags(signal_enabled_flags), d_flag_dump_enabled(flag_dump_to_file), - d_flag_dump_mat_enabled(flag_dump_to_mat) + d_flag_dump_mat_enabled(flag_dump_to_mat), + vtl_data(nullptr) { // see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc // function: satwavelen @@ -149,6 +150,12 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, } } } + + if (d_conf.enable_pvt_vtl == true) + { + vtl_data = std::make_unique(); + vtl_data->init_storage(d_conf.vtl_gps_channels + d_conf.vtl_gal_channels); + } } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h old mode 100644 new mode 100755 index 17c9000d8..e92390537 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -62,6 +62,7 @@ #include "rtklib.h" #include "rtklib_conversions.h" #include "sensor_data/sensor_data_aggregator.h" +#include "vtl_data.h" #include #include #include @@ -158,6 +159,9 @@ private: uint32_t d_signal_enabled_flags; bool d_flag_dump_enabled; bool d_flag_dump_mat_enabled; + + // vector tracking + std::unique_ptr vtl_data; }; diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc new file mode 100755 index 000000000..ccf106fe3 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -0,0 +1,67 @@ +/*! + * \file vtl_data.c + * \brief Class that exchange information to and from the Vector Tracking Loop (VTL) + * \author Pedro Pereira, 2025. pereirapedrocp@gmail.com + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#include "vtl_data.h" +#include "armadillo" +#include "vector" + +Vtl_Data::Vtl_Data() = default; + +void Vtl_Data::init_storage(int N_sv) +{ + rx_ch = arma::vec(N_sv); + rx_ch2 = arma::vec(N_sv); + rx_p = arma::mat(1, 3); + rx_v = arma::mat(1, 3); + rx_clk = arma::mat(1, 3); + + sv_id = arma::vec(N_sv); + sv_p = arma::mat(N_sv, 3); + sv_v = arma::mat(N_sv, 3); + sv_clk = arma::mat(N_sv, 2); + sv_elev = arma::vec(N_sv); + tropo_bias = arma::vec(N_sv); + iono_bias = arma::vec(N_sv); + code_bias = arma::vec(N_sv); + band = arma::vec(N_sv); + code_freq = arma::vec(N_sv); + CN0_dB_hz = arma::vec(N_sv); + ch_sample_counter = arma::vec(N_sv); + ch2_sample_counter = arma::vec(N_sv); + ionoopt = 0; + + obs_pr = arma::vec(N_sv); + obs_prr = arma::vec(N_sv); + + active_ch = arma::vec(N_sv); + past_active_ch = arma::vec(N_sv); + new_ch = arma::vec(N_sv); + old_ch = arma::vec(N_sv); + + loop_closure = arma::vec(N_sv); + + rx_time = 0.0; + dt_s = 0.0; +} + +void Vtl_Data::clear_storage() +{ + active_ch.zeros(); + new_ch.zeros(); + old_ch.zeros(); + loop_closure.zeros(); +} \ No newline at end of file diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h new file mode 100644 index 000000000..0d6bf0234 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -0,0 +1,86 @@ +/*! + * \file vtl_data.h + * \brief Class that exchange information to and from the Vector Tracking Loop (VTL) + * \author Pedro Pereira, 2025. pereirapedrocp@gmail.com + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_VTL_DATA_H +#define GNSS_SDR_VTL_DATA_H +// constants definition +constexpr double Lambda_GPS_L1 = 0.1902936728; +constexpr double Lambda_GPS_L5 = 0.2548280488; +constexpr double L1E1_CODE_FREQ = 1023000; +constexpr double L5E5_CODE_FREQ = 10230000; +constexpr double RANGE_TO_FREQ_L1E1_FACTOR = 0.0034123607; // L1E1_CODE_FREQ / SPEED_OF_LIGHT_M_S +constexpr double RANGE_TO_FREQ_L5E5_FACTOR = 0.0341236069; // L5E5_CODE_FREQ / SPEED_OF_LIGHT_M_S + +#include +#include +#include +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + + +class Vtl_Data +{ +public: + Vtl_Data(); + void init_storage(int N_sv); + void clear_storage(); + + arma::mat rx_ch; // receiver channel + arma::mat rx_ch2; // receiver channel - second frequency + arma::mat rx_p; // receiver position [m] + arma::mat rx_v; // receiver velocity [m/s] + arma::mat rx_clk; // receiver clock bias and drift [s,m/s] + + arma::mat sv_id; // satellite ID + arma::mat sv_p; // satellite position [m] + arma::mat sv_v; // satellite velocity [m/s] + arma::mat sv_clk; // satellite clock bias and drift [s,m/s] + arma::colvec sv_elev; // satellite elevation [rad] + arma::colvec tropo_bias; // troposphere bias [m] + arma::colvec iono_bias; // ionosphere bias [m] + arma::colvec code_bias; // code bias [m] + arma::colvec band; // frequency band + arma::colvec code_freq; + arma::colvec CN0_dB_hz; + arma::colvec ch_sample_counter; + arma::colvec ch2_sample_counter; + int ionoopt; // ionosphere option + + arma::colvec obs_pr; // observed pseudorange [m] + arma::colvec obs_prr; // observed pseudorange rate [m/s] + + arma::colvec active_ch; // active channels + arma::colvec past_active_ch; // past active channels + arma::colvec new_ch; // new channels + arma::colvec old_ch; // old channels + u_int8_t N_sv; // number of satellites for pvt + u_int8_t active_N_gps_ch; // active gps channels + u_int8_t active_N_gal_ch; // active gps channels + + arma::colvec loop_closure; // vtl loop closure + + double rx_time; + double dt_s; +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_VTL_DATA_H