mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-09-13 00:06:02 +00:00
Code Issues Releases Wiki Activity

mod: vtl_engine_get_geodetic

Browse Source
This commit is contained in:
M.A. Gomez
2024-01-19 17:22:50 +01:00
parent ed44787340
commit 0995d0bf30
4 changed files with 99 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

178
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@@ -2197,107 +2197,107 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
} }
// VTL (optional) MULTIPLEXED FILE RECORDING - Record results to file // 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<char *>(&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<char *>(&tmp_uint32), sizeof(uint32_t));
// PVT GPS time
tmp_double = gnss_observables_map.cbegin()->second.RX_time;
d_vtl_dump_file.write(reinterpret_cast<char *>(&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<char *>(&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<char *>(&tmp_double), sizeof(double));
try // rtklib User clock offset drift[s/s]
{ tmp_double = pvt_sol.dtr[5] / 1e6; // [ppm] to [s]
double tmp_double; d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(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<char *>(&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<char *>(&tmp_uint32), sizeof(uint32_t));
// PVT GPS time
tmp_double = gnss_observables_map.cbegin()->second.RX_time;
d_vtl_dump_file.write(reinterpret_cast<char *>(&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<char *>(&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<char *>(&tmp_double), sizeof(double));
// rtklib User clock offset drift[s/s] // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double)
tmp_double = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] std::vector<double> p_vec_m = vtl_engine.get_position_ecef_m();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); tmp_double = p_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&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<double> v_vec_m = vtl_engine.get_velocity_ecef_m_s();
std::vector<double> p_vec_m = vtl_engine.get_position_ecef_m(); tmp_double = v_vec_m[0];
tmp_double = p_vec_m[0]; d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); tmp_double = v_vec_m[1];
tmp_double = p_vec_m[1]; d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); tmp_double = v_vec_m[2];
tmp_double = p_vec_m[2]; d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> v_vec_m = vtl_engine.get_velocity_ecef_m_s(); std::vector<double> a_vec_m = vtl_engine.get_accel_ecef_m_s2();
tmp_double = v_vec_m[0]; tmp_double = a_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_vec_m[1]; tmp_double = a_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_vec_m[2]; tmp_double = a_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> a_vec_m = vtl_engine.get_accel_ecef_m_s2(); // position/velocity/acceleration variance/ (units^2) (9 x double)
tmp_double = a_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// position/velocity/acceleration variance/ (units^2) (9 x double) std::vector<double> p_var_vec_m = vtl_engine.get_position_var_ecef_m();
tmp_double = p_var_vec_m[0];
std::vector<double> p_var_vec_m = vtl_engine.get_position_var_ecef_m(); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_var_vec_m[0]; tmp_double = p_var_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_var_vec_m[1]; tmp_double = p_var_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = p_var_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
std::vector<double> v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s(); std::vector<double> v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s();
tmp_double = v_var_vec_m[0]; tmp_double = v_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_var_vec_m[1]; tmp_double = v_var_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = v_var_vec_m[2]; tmp_double = v_var_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
vector<double> a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2(); vector<double> a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2();
tmp_double = a_var_vec_m[0]; tmp_double = a_var_vec_m[0];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_var_vec_m[1]; tmp_double = a_var_vec_m[1];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = a_var_vec_m[2]; tmp_double = a_var_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// GEO user position Latitude [deg] // GEO user position Latitude [rad]
tmp_double = this->get_latitude(); vector<double> geo_vec_m = vtl_engine.get_geodetic_rad_m();
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); tmp_double = geo_vec_m[0];
// GEO user position Longitude [deg] d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = this->get_longitude(); // GEO user position Longitude [rad]
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); tmp_double = geo_vec_m[1];
// GEO user position Height [m] d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = this->get_height(); // GEO user position Height [m]
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); tmp_double = geo_vec_m[2];
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// NUMBER OF VALID SATS // NUMBER OF VALID SATS
d_vtl_dump_file.write(reinterpret_cast<char *>(&pvt_sol.ns), sizeof(uint8_t)); d_vtl_dump_file.write(reinterpret_cast<char *>(&pvt_sol.ns), sizeof(uint8_t));
// GDOP / PDOP / HDOP / VDOP // GDOP / PDOP / HDOP / VDOP
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[0]), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[0]), sizeof(double));
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[1]), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[1]), sizeof(double));
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[2]), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[2]), sizeof(double));
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[3]), sizeof(double)); d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[3]), sizeof(double));
} }
catch (const std::ofstream::failure &e) catch (const std::ofstream::failure &e)
{ {
LOG(WARNING) << "Exception writing VTL dump file " << e.what(); LOG(WARNING) << "Exception writing VTL dump file " << e.what();
} }
} }
} }
} }

1
src/algorithms/PVT/libs/vtl_conf.cc
View File

@@ -19,5 +19,4 @@
Vtl_Conf::Vtl_Conf() Vtl_Conf::Vtl_Conf()
{ {
} }

19
src/algorithms/PVT/libs/vtl_engine.cc
View File

@@ -15,6 +15,7 @@
*/ */
#include "vtl_engine.h" #include "vtl_engine.h"
#include "rtklib_rtkcmn.h"
#include "iostream" #include "iostream"
#include <fstream> #include <fstream>
@@ -442,19 +443,17 @@ std::vector<double> Vtl_Engine::get_accel_var_ecef_m_s2()
return temp; return temp;
} }
double Vtl_Engine::get_latitude() std::vector<double> Vtl_Engine::get_geodetic_rad_m()
{ {
return -1.0; std::array<double, 3> temp_ecef = {kf_x[0], kf_x[1], kf_x[2]};
} std::array<double, 3> temp_geo = {42, 42, 42};
double Vtl_Engine::get_longitude() ecef2pos(temp_ecef.data(), temp_geo.data());
{
return -1.0;
}
double Vtl_Engine::get_height() std::vector<double> dest;
{ dest.insert(dest.begin(), std::begin(temp_geo), std::end(temp_geo));
return -1.0;
return dest;
} }
double Vtl_Engine::get_user_clock_offset_s() double Vtl_Engine::get_user_clock_offset_s()

4
src/algorithms/PVT/libs/vtl_engine.h
View File

@@ -53,9 +53,7 @@ public:
std::vector<double> get_position_var_ecef_m(); // get_position_var_ecef_m std::vector<double> get_position_var_ecef_m(); // get_position_var_ecef_m
std::vector<double> get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s std::vector<double> get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s
std::vector<double> get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2 std::vector<double> get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2
double get_latitude(); // get_latitude std::vector<double> get_geodetic_rad_m(); // get_geodetic_rad_m
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_s(); // get_user_clock_offset_s;
double get_user_clock_offset_drift_s_s(); // get_user_clock_offset_drift_s/s; double get_user_clock_offset_drift_s_s(); // get_user_clock_offset_drift_s/s;