Add Vector Tracking Loop engine skeleton

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)

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)

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 <glog/logging.h>
@@ -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<int, Gnss_Synchro> &gnss_observables_map, bool flag_averaging)
+void get_vtl_data()
+{
+}
+
+bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, bool flag_averaging, bool get_vtl_data)
 {
     std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
     std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter;
@@ -997,6 +1010,63 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &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<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;
+
+                    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)

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 <array>
 #include <cstdint>
 #include <fstream>
@@ -84,7 +86,11 @@ 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_PVT(const std::map<int, Gnss_Synchro>& 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;
 };
 
 

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()
+{
+}

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 <cstdint>
+#include <map>
+#include <string>
+
+/** \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

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]");
+}

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 <armadillo>
+#include <cstdint>
+#include <map>
+#include <string>
+
+/** \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

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
+}

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 <armadillo>
+#include <cstdint>
+#include <string>
+
+/** \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