From ccf2b830a2fef375ee5c13d58c95e8c0707cc265 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Fri, 23 Sep 2022 12:28:19 +0200 Subject: [PATCH] Adding TrackingCmd class to VTL engine --- src/algorithms/PVT/libs/rtklib_solver.cc | 7 +------ src/algorithms/PVT/libs/rtklib_solver.h | 2 -- src/algorithms/PVT/libs/vtl_data.cc | 4 +++- src/algorithms/PVT/libs/vtl_data.h | 21 +++++++++++---------- src/algorithms/PVT/libs/vtl_engine.cc | 12 ++++++++++++ src/algorithms/PVT/libs/vtl_engine.h | 4 ++++ src/algorithms/libs/trackingcmd.h | 2 -- 7 files changed, 31 insertions(+), 21 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 2d17cb402..09a1d9007 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -465,11 +465,6 @@ Monitor_Pvt Rtklib_Solver::get_monitor_pvt() const return d_monitor_pvt; } - -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; @@ -1041,7 +1036,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ 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 for (int n = 0; n < n_sats; n++) { new_vtl_data.sat_p(n, 0) = rs[0 + 6 * n]; diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 1cc1323d5..b03cc886d 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -88,10 +88,8 @@ public: 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; double get_pdop() const override; diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index 4718ad7b4..25824b0cf 100644 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -20,6 +20,7 @@ Vtl_Data::Vtl_Data() { epoch_tow_s = 0; + sample_counter = 0; } void Vtl_Data::init_storage(int n_sats) @@ -33,11 +34,12 @@ void Vtl_Data::init_storage(int n_sats) doppler_hz = arma::vec(n_sats); carrier_phase_rads = arma::vec(n_sats); epoch_tow_s = 0; + sample_counter = 0; } void Vtl_Data::debug_print() { - std::cout << "vtl_data debug print at TOW: " << epoch_tow_s << "\n "; + std::cout << "vtl_data debug print at RX TOW: " << epoch_tow_s << ", TRK sample counter: " << sample_counter << "\n"; sat_p.print("VTL Sat Positions"); sat_v.print("VTL Sat Velocities"); sat_dts.print("VTL Sat clocks"); diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index c4de5c737..8507f6d1d 100644 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -19,7 +19,6 @@ #include #include -#include #include /** \addtogroup PVT @@ -34,17 +33,19 @@ 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::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] + 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] + // time handling + double epoch_tow_s; // current observation RX time [s] + uint64_t sample_counter; // current sample counter associated with RX time [samples from start] void debug_print(); }; diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index b51fd641e..31a8781e7 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -27,6 +27,18 @@ Vtl_Engine::~Vtl_Engine() bool Vtl_Engine::vtl_loop(Vtl_Data new_data) { //TODO: Implement main VTL loop here + + //TODO: Fill the tracking commands outputs + // Notice: keep the same satellite order as in the Vtl_Data matrices + // sample code + TrackingCmd trk_cmd; + trk_cmd.carrier_freq_hz = 0; + trk_cmd.carrier_freq_rate_hz_s = 0; + trk_cmd.code_freq_chips = 0; + trk_cmd.enable_carrier_nco_cmd = true; + trk_cmd.enable_code_nco_cmd = true; + trk_cmd.sample_counter = new_data.sample_counter; + trk_cmd_outs.push_back(trk_cmd); return true; } diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index ed972f058..ac8a750df 100644 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -17,11 +17,13 @@ #ifndef GNSS_SDR_VTL_ENGINE_H #define GNSS_SDR_VTL_ENGINE_H +#include "trackingcmd.h" #include "vtl_conf.h" #include "vtl_data.h" #include #include #include +#include /** \addtogroup PVT * \{ */ @@ -43,6 +45,8 @@ public: void reset(); // reset all internal states void debug_print(); // print debug information + std::vector trk_cmd_outs; // vector holding the Tracking command states updates to be sent to tracking KFs + private: Vtl_Conf config; //TODO: Internal VTL persistent variables here diff --git a/src/algorithms/libs/trackingcmd.h b/src/algorithms/libs/trackingcmd.h index cf56ed5f8..77115167b 100644 --- a/src/algorithms/libs/trackingcmd.h +++ b/src/algorithms/libs/trackingcmd.h @@ -28,8 +28,6 @@ class TrackingCmd { public: - TrackingCmd(); - bool enable_carrier_nco_cmd = false; bool enable_code_nco_cmd = false; double code_freq_chips = 0.0;