diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 61f141ee8..bb07f6558 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2131,7 +2131,8 @@ 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 - if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false)) + bool get_vtl_data = true; + if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false, get_vtl_data)) { d_pvt_errors_counter = 0; // Reset consecutive PVT error counter const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s(); @@ -2245,7 +2246,7 @@ 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); + flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false); } if (flag_pvt_valid == true) diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index 8e2785144..83c708f46 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -22,6 +22,9 @@ set(PVT_LIB_SOURCES monitor_pvt_udp_sink.cc monitor_ephemeris_udp_sink.cc has_simple_printer.cc + vtl_conf.cc + vtl_data.cc + vtl_engine.cc ) set(PVT_LIB_HEADERS @@ -43,6 +46,9 @@ set(PVT_LIB_HEADERS serdes_gps_eph.h monitor_ephemeris_udp_sink.h has_simple_printer.h + vtl_conf.h + vtl_data.h + vtl_engine.h ) list(SORT PVT_LIB_HEADERS) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index bab77a024..2d17cb402 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -34,6 +34,7 @@ #include "Beidou_DNAV.h" #include "gnss_sdr_filesystem.h" #include "rtklib_conversions.h" +#include "rtklib_ephemeris.h" #include "rtklib_rtkpos.h" #include "rtklib_solution.h" #include @@ -55,6 +56,14 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, d_flag_dump_mat_enabled(flag_dump_to_mat), d_use_e6_for_pvt(use_e6_for_pvt) { + // TODO: temporal VTL config parameters are hardcoded here. Move it to PVT adapter config (javi) + Vtl_Conf new_vtl_conf; + //TODO: new_vtl_conf.parameter1=blablabla + + vtl_engine.configure(new_vtl_conf); + vtl_engine.reset(); + + this->set_averaging_flag(false); // see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -457,7 +466,11 @@ Monitor_Pvt Rtklib_Solver::get_monitor_pvt() const } -bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging) +void get_vtl_data() +{ +} + +bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging, bool get_vtl_data) { std::map::const_iterator gnss_observables_iter; std::map::const_iterator galileo_ephemeris_iter; @@ -997,6 +1010,63 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } } + + 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; + + std::vector 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; + + 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); + + // 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]; + } + new_vtl_data.debug_print(); + + //Call the VTL engine loop + vtl_engine.vtl_loop(new_vtl_data); + } + result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data); if (result == 0) diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 22d20ba2f..1cc1323d5 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -57,6 +57,8 @@ #include "monitor_pvt.h" #include "pvt_solution.h" #include "rtklib.h" +#include "vtl_data.h" +#include "vtl_engine.h" #include #include #include @@ -84,7 +86,11 @@ public: bool use_e6_for_pvt = true); ~Rtklib_Solver(); - bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging); + bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging, bool get_vtl_data); + + void get_vtl_data(); + Vtl_Data vtl_data; + double get_hdop() const override; double get_vdop() const override; @@ -135,6 +141,7 @@ private: bool d_flag_dump_enabled; bool d_flag_dump_mat_enabled; bool d_use_e6_for_pvt; + Vtl_Engine vtl_engine; }; diff --git a/src/algorithms/PVT/libs/vtl_conf.cc b/src/algorithms/PVT/libs/vtl_conf.cc new file mode 100644 index 000000000..6353ad7f3 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_conf.cc @@ -0,0 +1,22 @@ +/*! + * \file vtl_conf.c + * \brief Class that contains all the parameters to configure the Vector Tracking Loop (VTL) Kalman filter engine + * \author Javier Arribas, 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * 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_conf.h" + +Vtl_Conf::Vtl_Conf() +{ +} diff --git a/src/algorithms/PVT/libs/vtl_conf.h b/src/algorithms/PVT/libs/vtl_conf.h new file mode 100644 index 000000000..eb5bfadb3 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_conf.h @@ -0,0 +1,40 @@ +/*! + * \file vtl_conf.h + * \brief Class that contains all the parameters to configure the Vector Tracking Loop (VTL) Kalman filter engine + * \author Javier Arribas, 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * 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_CONF_H +#define GNSS_SDR_VTL_CONF_H + +#include +#include +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + + +class Vtl_Conf +{ +public: + Vtl_Conf(); + //TODO: VTL control parameters and config options here +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_VTL_CONF_H diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc new file mode 100644 index 000000000..4718ad7b4 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -0,0 +1,50 @@ +/*! + * \file vtl_data.c + * \brief Class that exchange information to and from the Vector Tracking Loop (VTL) Kalman filter engine + * \author Javier Arribas, 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * 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" + +Vtl_Data::Vtl_Data() +{ + epoch_tow_s = 0; +} + +void Vtl_Data::init_storage(int n_sats) +{ + sat_p = arma::mat(n_sats, 3); + sat_v = arma::mat(n_sats, 3); + sat_dts = arma::mat(n_sats, 2); + sat_var = arma::vec(n_sats); + sat_health_flag = arma::vec(n_sats); + pr_m = arma::vec(n_sats); + doppler_hz = arma::vec(n_sats); + carrier_phase_rads = arma::vec(n_sats); + epoch_tow_s = 0; +} + +void Vtl_Data::debug_print() +{ + std::cout << "vtl_data debug print at TOW: " << epoch_tow_s << "\n "; + sat_p.print("VTL Sat Positions"); + sat_v.print("VTL Sat Velocities"); + sat_dts.print("VTL Sat clocks"); + sat_var.print("VTL Sat clock variances"); + sat_health_flag.print("VTL Sat health"); + + pr_m.print("Satellite Code pseudoranges [m]"); + doppler_hz.print("satellite Carrier Dopplers [Hz]"); + carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); +} diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h new file mode 100644 index 000000000..c4de5c737 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -0,0 +1,54 @@ +/*! + * \file vtl_data.h + * \brief Class that exchange information to and from the Vector Tracking Loop (VTL) Kalman filter engine + * \author Javier Arribas, 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * 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 + +#include +#include +#include +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + + +class Vtl_Data +{ +public: + Vtl_Data(); + void init_storage(int n_sats); + + arma::mat sat_p; //Satellite ECEF Position [m] + arma::mat sat_v; //Satellite Velocity [m/s] + arma::mat sat_dts; //Satellite clock bias and drift [s,s/s] + arma::vec sat_var; //sat position and clock error variance [m^2] + arma::vec sat_health_flag; //sat health flag (0 is ok) + + arma::vec pr_m; //Satellite Code pseudoranges [m] + arma::vec doppler_hz; //satellite Carrier Dopplers [Hz] + arma::vec carrier_phase_rads; //satellite accumulated carrier phases [rads] + + double epoch_tow_s; //current observation RX time [s] + void debug_print(); +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_VTL_DATA_H diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc new file mode 100644 index 000000000..b51fd641e --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -0,0 +1,47 @@ +/*! + * \file vtl_engine.h + * \brief Class that implements a Vector Tracking Loop (VTL) Kalman filter engine + * \author Javier Arribas, 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * 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_engine.h" + +Vtl_Engine::Vtl_Engine() +{ +} + +Vtl_Engine::~Vtl_Engine() +{ +} + +bool Vtl_Engine::vtl_loop(Vtl_Data new_data) +{ + //TODO: Implement main VTL loop here + return true; +} + +void Vtl_Engine::reset() +{ + //TODO +} + +void Vtl_Engine::debug_print() +{ + //TODO +} + +void Vtl_Engine::configure(Vtl_Conf config_) +{ + config = config_; + //TODO: initialize internal variables +} diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h new file mode 100644 index 000000000..ed972f058 --- /dev/null +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -0,0 +1,54 @@ +/*! + * \file vtl_engine.h + * \brief Class that implements a Vector Tracking Loop (VTL) Kalman filter engine + * \author Javier Arribas, 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * 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_ENGINE_H +#define GNSS_SDR_VTL_ENGINE_H + +#include "vtl_conf.h" +#include "vtl_data.h" +#include +#include +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + + +class Vtl_Engine +{ +public: + Vtl_Engine(); + + ~Vtl_Engine(); + + void configure(Vtl_Conf config_); //set config parameters + + //TODO: output functions here (output for tracking KF updates, VTL computed user PVT, etc...) + bool vtl_loop(Vtl_Data new_data); + void reset(); // reset all internal states + void debug_print(); // print debug information + +private: + Vtl_Conf config; + //TODO: Internal VTL persistent variables here +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_VTL_ENGINE_H