mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-01-10 01:10:34 +00:00
mod: vtl_engine_get_geodetic
This commit is contained in:
M.A. Gomez
parent
ed44787340
commit
0995d0bf30
@ -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
|
||||
//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
|
||||
{
|
||||
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));
|
||||
// rtklib User clock offset drift[s/s]
|
||||
tmp_double = pvt_sol.dtr[5] / 1e6; // [ppm] to [s]
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&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<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> p_vec_m = vtl_engine.get_position_ecef_m();
|
||||
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> p_vec_m = vtl_engine.get_position_ecef_m();
|
||||
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));
|
||||
std::vector<double> v_vec_m = vtl_engine.get_velocity_ecef_m_s();
|
||||
tmp_double = v_vec_m[0];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = v_vec_m[1];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = v_vec_m[2];
|
||||
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();
|
||||
tmp_double = v_vec_m[0];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = v_vec_m[1];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = v_vec_m[2];
|
||||
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();
|
||||
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));
|
||||
|
||||
std::vector<double> a_vec_m = vtl_engine.get_accel_ecef_m_s2();
|
||||
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)
|
||||
|
||||
// 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];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = p_var_vec_m[1];
|
||||
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> 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<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = p_var_vec_m[1];
|
||||
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();
|
||||
tmp_double = v_var_vec_m[0];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = v_var_vec_m[1];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = v_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();
|
||||
tmp_double = v_var_vec_m[0];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = v_var_vec_m[1];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = v_var_vec_m[2];
|
||||
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();
|
||||
tmp_double = a_var_vec_m[0];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = a_var_vec_m[1];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = a_var_vec_m[2];
|
||||
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();
|
||||
tmp_double = a_var_vec_m[0];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = a_var_vec_m[1];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
tmp_double = a_var_vec_m[2];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = this->get_latitude();
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = this->get_longitude();
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = this->get_height();
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
// GEO user position Latitude [rad]
|
||||
vector<double> geo_vec_m = vtl_engine.get_geodetic_rad_m();
|
||||
tmp_double = geo_vec_m[0];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
// GEO user position Longitude [rad]
|
||||
tmp_double = geo_vec_m[1];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = geo_vec_m[2];
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
|
||||
// NUMBER OF VALID SATS
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&pvt_sol.ns), sizeof(uint8_t));
|
||||
// NUMBER OF VALID SATS
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&pvt_sol.ns), sizeof(uint8_t));
|
||||
|
||||
// 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[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[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<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[2]), sizeof(double));
|
||||
d_vtl_dump_file.write(reinterpret_cast<char *>(&d_dop[3]), sizeof(double));
|
||||
}
|
||||
catch (const std::ofstream::failure &e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing VTL dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -19,5 +19,4 @@
|
||||
|
||||
Vtl_Conf::Vtl_Conf()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
@ -15,6 +15,7 @@
|
||||
*/
|
||||
|
||||
#include "vtl_engine.h"
|
||||
#include "rtklib_rtkcmn.h"
|
||||
#include "iostream"
|
||||
#include <fstream>
|
||||
|
||||
@ -442,19 +443,17 @@ std::vector<double> Vtl_Engine::get_accel_var_ecef_m_s2()
|
||||
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()
|
||||
{
|
||||
return -1.0;
|
||||
}
|
||||
ecef2pos(temp_ecef.data(), temp_geo.data());
|
||||
|
||||
double Vtl_Engine::get_height()
|
||||
{
|
||||
return -1.0;
|
||||
std::vector<double> dest;
|
||||
dest.insert(dest.begin(), std::begin(temp_geo), std::end(temp_geo));
|
||||
|
||||
return dest;
|
||||
}
|
||||
|
||||
double Vtl_Engine::get_user_clock_offset_s()
|
||||
|
||||
@ -53,9 +53,7 @@ public:
|
||||
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_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<double> 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;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user