From 2f17b5c5e0a371efe311186f7c86dc7573872224 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 5 Apr 2023 12:06:17 +0200 Subject: [PATCH] ADD: user clock drift to PVT_raw dump file --- src/algorithms/PVT/libs/rtklib_solver.cc | 12 ++++++++++-- src/algorithms/PVT/libs/vtl_engine.cc | 8 ++++++++ src/algorithms/PVT/libs/vtl_engine.h | 1 + 3 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 7dbc2f711..2af55de3e 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -236,7 +236,7 @@ bool Rtklib_Solver::save_vtl_matfile() const { // READ DUMP FILE const std::string dump_filename = d_vtl_dump_filename; - const int32_t number_of_double_vars = 27; + const int32_t number_of_double_vars = 28; const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 1; const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + @@ -271,6 +271,7 @@ bool Rtklib_Solver::save_vtl_matfile() const auto week = std::vector(num_epoch); auto RX_time = std::vector(num_epoch); auto user_clk_offset = std::vector(num_epoch); + auto user_clk_offset_drift = std::vector(num_epoch); auto pos_x = std::vector(num_epoch); auto pos_y = std::vector(num_epoch); auto pos_z = std::vector(num_epoch); @@ -308,6 +309,7 @@ bool Rtklib_Solver::save_vtl_matfile() const dump_file.read(reinterpret_cast(&week[i]), sizeof(uint32_t)); dump_file.read(reinterpret_cast(&RX_time[i]), sizeof(double)); dump_file.read(reinterpret_cast(&user_clk_offset[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&user_clk_offset_drift[i]), sizeof(double)); dump_file.read(reinterpret_cast(&pos_x[i]), sizeof(double)); dump_file.read(reinterpret_cast(&pos_y[i]), sizeof(double)); dump_file.read(reinterpret_cast(&pos_z[i]), sizeof(double)); @@ -370,6 +372,10 @@ bool Rtklib_Solver::save_vtl_matfile() const Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("user_clk_offset_drift", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), user_clk_offset_drift.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + matvar = Mat_VarCreate("pos_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_x.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -2097,9 +2103,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = gnss_observables_map.cbegin()->second.RX_time; d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // User clock offset [s] - //tmp_double = rx_position_and_time[3]; tmp_double = vtl_engine.get_user_clock_offset_s(); d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // User clock offset drift[s/s] + tmp_double = vtl_engine.get_user_clock_offset_drift_s_s(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double) std::vector p_vec_m = vtl_engine.get_position_ecef_m(); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 808dbe77d..14f08b1d3 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -3276,5 +3276,13 @@ double Vtl_Engine::get_user_clock_offset_s() double temp = 0; temp = kf_x[9]; + return temp; +} + +double Vtl_Engine::get_user_clock_offset_drift_s_s() +{ + double temp = 0; + temp = kf_x[10]; + return temp; } \ No newline at end of file diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index ec5db6fc9..98ad23931 100644 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -57,6 +57,7 @@ public: double get_longitude(); // get_longitude double get_height(); // get_height double get_user_clock_offset_s(); // get_user_clock_offset_s; + double get_user_clock_offset_drift_s_s(); // get_user_clock_offset_drift_s/s; private: Vtl_Conf config;