From 0995d0bf3012949e56bda3a162d1e706f1f4ab40 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Fri, 19 Jan 2024 17:22:50 +0100 Subject: [PATCH] mod: vtl_engine_get_geodetic --- src/algorithms/PVT/libs/rtklib_solver.cc | 178 +++++++++++------------ src/algorithms/PVT/libs/vtl_conf.cc | 1 - src/algorithms/PVT/libs/vtl_engine.cc | 19 ++- src/algorithms/PVT/libs/vtl_engine.h | 4 +- 4 files changed, 99 insertions(+), 103 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 1c6a15158..d9c90050b 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -2197,107 +2197,107 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } // VTL (optional) MULTIPLEXED FILE RECORDING - Record results to file - //if (enable_vtl == true) + // if (enable_vtl == true) + try + { + double tmp_double; + uint32_t tmp_uint32; + // TOW + tmp_uint32 = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms; + d_vtl_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + // WEEK + tmp_uint32 = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009()); + d_vtl_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + // PVT GPS time + tmp_double = gnss_observables_map.cbegin()->second.RX_time; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // VTL filtered User clock offset [s] + tmp_double = vtl_engine.get_user_clock_offset_s(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // VTL filtered 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)); - try - { - double tmp_double; - uint32_t tmp_uint32; - // TOW - tmp_uint32 = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms; - d_vtl_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); - // WEEK - tmp_uint32 = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009()); - d_vtl_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); - // PVT GPS time - tmp_double = gnss_observables_map.cbegin()->second.RX_time; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // VTL filtered User clock offset [s] - tmp_double = vtl_engine.get_user_clock_offset_s(); - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // VTL filtered 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)); + // rtklib User clock offset drift[s/s] + tmp_double = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // rtklib User clock offset drift[s/s] - tmp_double = pvt_sol.dtr[5] / 1e6; // [ppm] to [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(); + tmp_double = p_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_vec_m[2]; + 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(); - tmp_double = p_vec_m[0]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = p_vec_m[1]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = p_vec_m[2]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + std::vector v_vec_m = vtl_engine.get_velocity_ecef_m_s(); + tmp_double = v_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector v_vec_m = vtl_engine.get_velocity_ecef_m_s(); - tmp_double = v_vec_m[0]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = v_vec_m[1]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = v_vec_m[2]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + std::vector a_vec_m = vtl_engine.get_accel_ecef_m_s2(); + tmp_double = a_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector a_vec_m = vtl_engine.get_accel_ecef_m_s2(); - tmp_double = a_vec_m[0]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = a_vec_m[1]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = a_vec_m[2]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // position/velocity/acceleration variance/ (units^2) (9 x double) - // position/velocity/acceleration variance/ (units^2) (9 x double) - - std::vector p_var_vec_m = vtl_engine.get_position_var_ecef_m(); - tmp_double = p_var_vec_m[0]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = p_var_vec_m[1]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = p_var_vec_m[2]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + std::vector p_var_vec_m = vtl_engine.get_position_var_ecef_m(); + tmp_double = p_var_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_var_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_var_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - std::vector v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s(); - tmp_double = v_var_vec_m[0]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = v_var_vec_m[1]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = v_var_vec_m[2]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + std::vector v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s(); + tmp_double = v_var_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_var_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_var_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - vector a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2(); - tmp_double = a_var_vec_m[0]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = a_var_vec_m[1]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = a_var_vec_m[2]; - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + vector a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2(); + tmp_double = a_var_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_var_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_var_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // GEO user position Latitude [deg] - tmp_double = this->get_latitude(); - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // GEO user position Longitude [deg] - tmp_double = this->get_longitude(); - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // GEO user position Height [m] - tmp_double = this->get_height(); - d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // GEO user position Latitude [rad] + vector geo_vec_m = vtl_engine.get_geodetic_rad_m(); + tmp_double = geo_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // GEO user position Longitude [rad] + tmp_double = geo_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // GEO user position Height [m] + tmp_double = geo_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // NUMBER OF VALID SATS - d_vtl_dump_file.write(reinterpret_cast(&pvt_sol.ns), sizeof(uint8_t)); + // NUMBER OF VALID SATS + d_vtl_dump_file.write(reinterpret_cast(&pvt_sol.ns), sizeof(uint8_t)); - // GDOP / PDOP / HDOP / VDOP - d_vtl_dump_file.write(reinterpret_cast(&d_dop[0]), sizeof(double)); - d_vtl_dump_file.write(reinterpret_cast(&d_dop[1]), sizeof(double)); - d_vtl_dump_file.write(reinterpret_cast(&d_dop[2]), sizeof(double)); - d_vtl_dump_file.write(reinterpret_cast(&d_dop[3]), sizeof(double)); - } - catch (const std::ofstream::failure &e) - { - LOG(WARNING) << "Exception writing VTL dump file " << e.what(); - } + // GDOP / PDOP / HDOP / VDOP + d_vtl_dump_file.write(reinterpret_cast(&d_dop[0]), sizeof(double)); + d_vtl_dump_file.write(reinterpret_cast(&d_dop[1]), sizeof(double)); + d_vtl_dump_file.write(reinterpret_cast(&d_dop[2]), sizeof(double)); + d_vtl_dump_file.write(reinterpret_cast(&d_dop[3]), sizeof(double)); + } + catch (const std::ofstream::failure &e) + { + LOG(WARNING) << "Exception writing VTL dump file " << e.what(); + } } } } diff --git a/src/algorithms/PVT/libs/vtl_conf.cc b/src/algorithms/PVT/libs/vtl_conf.cc index b685d7db1..6353ad7f3 100644 --- a/src/algorithms/PVT/libs/vtl_conf.cc +++ b/src/algorithms/PVT/libs/vtl_conf.cc @@ -19,5 +19,4 @@ Vtl_Conf::Vtl_Conf() { - } diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index cbfecb786..562862f86 100644 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -15,6 +15,7 @@ */ #include "vtl_engine.h" +#include "rtklib_rtkcmn.h" #include "iostream" #include @@ -442,19 +443,17 @@ std::vector Vtl_Engine::get_accel_var_ecef_m_s2() return temp; } -double Vtl_Engine::get_latitude() +std::vector Vtl_Engine::get_geodetic_rad_m() { - return -1.0; -} + std::array temp_ecef = {kf_x[0], kf_x[1], kf_x[2]}; + std::array temp_geo = {42, 42, 42}; -double Vtl_Engine::get_longitude() -{ - return -1.0; -} + ecef2pos(temp_ecef.data(), temp_geo.data()); -double Vtl_Engine::get_height() -{ - return -1.0; + std::vector dest; + dest.insert(dest.begin(), std::begin(temp_geo), std::end(temp_geo)); + + return dest; } double Vtl_Engine::get_user_clock_offset_s() diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 122d51041..d07f31c3f 100644 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -53,9 +53,7 @@ public: std::vector get_position_var_ecef_m(); // get_position_var_ecef_m std::vector get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s std::vector get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2 - double get_latitude(); // get_latitude - double get_longitude(); // get_longitude - double get_height(); // get_height + std::vector get_geodetic_rad_m(); // get_geodetic_rad_m 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;