mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 04:00:34 +00:00
Pass map of observables by reference
This commit is contained in:
Carles Fernandez
parent
07113fd89d
commit
6c19437520
@ -74,33 +74,43 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
|
||||
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_flag_dump_enabled == true)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
|
||||
}
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
rtklib_solver::~rtklib_solver()
|
||||
{
|
||||
d_dump_file.close();
|
||||
if (d_dump_file.is_open() == true)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_file.close();
|
||||
}
|
||||
catch(const std::exception & ex)
|
||||
{
|
||||
LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool rtklib_solver::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, double Rx_time, bool flag_averaging)
|
||||
bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_map, double Rx_time, bool flag_averaging)
|
||||
{
|
||||
std::map<int,Gnss_Synchro>::iterator gnss_observables_iter;
|
||||
std::map<int,Gnss_Synchro>::const_iterator gnss_observables_iter;
|
||||
std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter;
|
||||
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
|
||||
std::map<int,Gps_CNAV_Ephemeris>::iterator gps_cnav_ephemeris_iter;
|
||||
@ -115,246 +125,245 @@ bool rtklib_solver::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
|
||||
obsd_t obs_data[MAXOBS];
|
||||
eph_t eph_data[MAXOBS];
|
||||
|
||||
for(gnss_observables_iter = gnss_observables_map.begin();
|
||||
gnss_observables_iter != gnss_observables_map.end();
|
||||
for(gnss_observables_iter = gnss_observables_map.cbegin();
|
||||
gnss_observables_iter != gnss_observables_map.cend();
|
||||
gnss_observables_iter++)
|
||||
{
|
||||
switch(gnss_observables_iter->second.System)
|
||||
{
|
||||
case 'E':
|
||||
{
|
||||
std::string sig_(gnss_observables_iter->second.Signal);
|
||||
// Galileo E1
|
||||
if(sig_.compare("1B") == 0)
|
||||
switch(gnss_observables_iter->second.System)
|
||||
{
|
||||
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
|
||||
case 'E':
|
||||
{
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
galileo_ephemeris_iter->second.WN_5,
|
||||
0);
|
||||
valid_obs++;
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
|
||||
}
|
||||
// Galileo E5
|
||||
if(sig_.compare("5X") == 0)
|
||||
{
|
||||
|
||||
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
|
||||
{
|
||||
bool found_E1_obs=false;
|
||||
for (int i = 0; i < valid_obs; i++)
|
||||
{
|
||||
if (eph_data[i].sat == (static_cast<int>(gnss_observables_iter->second.PRN+NSATGPS+NSATGLO)))
|
||||
std::string sig_(gnss_observables_iter->second.Signal);
|
||||
// Galileo E1
|
||||
if(sig_.compare("1B") == 0)
|
||||
{
|
||||
obs_data[i] = insert_obs_to_rtklib(obs_data[i],
|
||||
gnss_observables_iter->second,
|
||||
galileo_ephemeris_iter->second.WN_5,
|
||||
2);//Band 3 (L5/E5)
|
||||
found_E1_obs=true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found_E1_obs)
|
||||
{
|
||||
//insert Galileo E5 obs as new obs and also insert its ephemeris
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
galileo_ephemeris_iter->second.WN_5,
|
||||
2); //Band 3 (L5/E5)
|
||||
valid_obs++;
|
||||
}
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
|
||||
{
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
galileo_ephemeris_iter->second.WN_5,
|
||||
0);
|
||||
valid_obs++;
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 'G':
|
||||
{
|
||||
// GPS L1
|
||||
// 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key
|
||||
std::string sig_(gnss_observables_iter->second.Signal);
|
||||
if(sig_.compare("1C") == 0)
|
||||
{
|
||||
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (gps_ephemeris_iter != gps_ephemeris_map.end())
|
||||
{
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
gps_ephemeris_iter->second.i_GPS_week,
|
||||
0);
|
||||
valid_obs++;
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
|
||||
}
|
||||
}
|
||||
//GPS L2
|
||||
if(sig_.compare("2S") == 0)
|
||||
{
|
||||
gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end())
|
||||
{
|
||||
// 1. Find the same satellite in GPS L1 band
|
||||
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (gps_ephemeris_iter != gps_ephemeris_map.end())
|
||||
{
|
||||
// 2. If found, replace the existing GPS L1 ephemeris with the GPS L2 ephemeris
|
||||
// (more precise!), and attach the L2 observation to the L1 observation in RTKLIB structure
|
||||
for (int i = 0; i < valid_obs; i++)
|
||||
}
|
||||
// Galileo E5
|
||||
if(sig_.compare("5X") == 0)
|
||||
{
|
||||
if (eph_data[i].sat == static_cast<int>(gnss_observables_iter->second.PRN))
|
||||
{
|
||||
eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
|
||||
obs_data[i] = insert_obs_to_rtklib(obs_data[i],
|
||||
gnss_observables_iter->second,
|
||||
gps_cnav_ephemeris_iter->second.i_GPS_week,
|
||||
1);//Band 2 (L2)
|
||||
break;
|
||||
}
|
||||
|
||||
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
|
||||
{
|
||||
bool found_E1_obs=false;
|
||||
for (int i = 0; i < valid_obs; i++)
|
||||
{
|
||||
if (eph_data[i].sat == (static_cast<int>(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO)))
|
||||
{
|
||||
obs_data[i] = insert_obs_to_rtklib(obs_data[i],
|
||||
gnss_observables_iter->second,
|
||||
galileo_ephemeris_iter->second.WN_5,
|
||||
2);//Band 3 (L5/E5)
|
||||
found_E1_obs=true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found_E1_obs)
|
||||
{
|
||||
//insert Galileo E5 obs as new obs and also insert its ephemeris
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
galileo_ephemeris_iter->second.WN_5,
|
||||
2); //Band 3 (L5/E5)
|
||||
valid_obs++;
|
||||
}
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 3. If not found, insert the GPS L2 ephemeris and the observation
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
gps_cnav_ephemeris_iter->second.i_GPS_week,
|
||||
1);//Band 2 (L2)
|
||||
valid_obs++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
case 'G':
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
// GPS L1
|
||||
// 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key
|
||||
std::string sig_(gnss_observables_iter->second.Signal);
|
||||
if(sig_.compare("1C") == 0)
|
||||
{
|
||||
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (gps_ephemeris_iter != gps_ephemeris_map.end())
|
||||
{
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
gps_ephemeris_iter->second.i_GPS_week,
|
||||
0);
|
||||
valid_obs++;
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
|
||||
}
|
||||
}
|
||||
//GPS L2
|
||||
if(sig_.compare("2S") == 0)
|
||||
{
|
||||
gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end())
|
||||
{
|
||||
// 1. Find the same satellite in GPS L1 band
|
||||
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (gps_ephemeris_iter != gps_ephemeris_map.end())
|
||||
{
|
||||
// 2. If found, replace the existing GPS L1 ephemeris with the GPS L2 ephemeris
|
||||
// (more precise!), and attach the L2 observation to the L1 observation in RTKLIB structure
|
||||
for (int i = 0; i < valid_obs; i++)
|
||||
{
|
||||
if (eph_data[i].sat == static_cast<int>(gnss_observables_iter->second.PRN))
|
||||
{
|
||||
eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
|
||||
obs_data[i] = insert_obs_to_rtklib(obs_data[i],
|
||||
gnss_observables_iter->second,
|
||||
gps_cnav_ephemeris_iter->second.i_GPS_week,
|
||||
1);//Band 2 (L2)
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 3. If not found, insert the GPS L2 ephemeris and the observation
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
gps_cnav_ephemeris_iter->second.i_GPS_week,
|
||||
1);//Band 2 (L2)
|
||||
valid_obs++;
|
||||
}
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
default :
|
||||
DLOG(INFO) << "Hybrid observables: Unknown GNSS";
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default :
|
||||
DLOG(INFO) << "Hybrid observables: Unknown GNSS";
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// **********************************************************************
|
||||
// ****** SOLVE PVT******************************************************
|
||||
// **********************************************************************
|
||||
// **********************************************************************
|
||||
// ****** SOLVE PVT******************************************************
|
||||
// **********************************************************************
|
||||
|
||||
b_valid_position = false;
|
||||
if (valid_obs > 0)
|
||||
b_valid_position = false;
|
||||
if (valid_obs > 0)
|
||||
{
|
||||
int result = 0;
|
||||
nav_t nav_data;
|
||||
nav_data.eph = eph_data;
|
||||
nav_data.n = valid_obs;
|
||||
for (int i = 0; i < MAXSAT; i++)
|
||||
{
|
||||
nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1; /* L1/E1 */
|
||||
nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; /* L2 */
|
||||
nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; /* L5/E5 */
|
||||
}
|
||||
{
|
||||
nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1; /* L1/E1 */
|
||||
nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; /* L2 */
|
||||
nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; /* L5/E5 */
|
||||
}
|
||||
|
||||
result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data);
|
||||
if(result == 0)
|
||||
{
|
||||
LOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf;
|
||||
d_rx_dt_s = 0; //reset rx time estimation
|
||||
d_valid_observations = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_valid_observations = rtk_.sol.ns; //record the number of valid satellites used by the PVT solver
|
||||
pvt_sol = rtk_.sol;
|
||||
b_valid_position = true;
|
||||
arma::vec rx_position_and_time(4);
|
||||
rx_position_and_time(0) = pvt_sol.rr[0];
|
||||
rx_position_and_time(1) = pvt_sol.rr[1];
|
||||
rx_position_and_time(2) = pvt_sol.rr[2];
|
||||
rx_position_and_time(3) = pvt_sol.dtr[0];
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
|
||||
DLOG(INFO) << "RTKLIB Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
|
||||
|
||||
boost::posix_time::ptime p_time;
|
||||
gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time);
|
||||
p_time = boost::posix_time::from_time_t(rtklib_utc_time.time);
|
||||
p_time+=boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6));
|
||||
d_position_UTC_time = p_time;
|
||||
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
|
||||
|
||||
DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time)
|
||||
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
|
||||
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
|
||||
|
||||
// ######## LOG FILE #########
|
||||
if(d_flag_dump_enabled == true)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double = Rx_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = rx_position_and_time(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = rx_position_and_time(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = rx_position_and_time(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = rx_position_and_time(3);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
|
||||
}
|
||||
LOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf;
|
||||
d_rx_dt_s = 0; //reset rx time estimation
|
||||
d_valid_observations = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_valid_observations = rtk_.sol.ns; //record the number of valid satellites used by the PVT solver
|
||||
pvt_sol = rtk_.sol;
|
||||
b_valid_position = true;
|
||||
arma::vec rx_position_and_time(4);
|
||||
rx_position_and_time(0) = pvt_sol.rr[0];
|
||||
rx_position_and_time(1) = pvt_sol.rr[1];
|
||||
rx_position_and_time(2) = pvt_sol.rr[2];
|
||||
rx_position_and_time(3) = pvt_sol.dtr[0];
|
||||
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
|
||||
d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
|
||||
DLOG(INFO) << "RTKLIB Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
|
||||
|
||||
boost::posix_time::ptime p_time;
|
||||
gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time);
|
||||
p_time = boost::posix_time::from_time_t(rtklib_utc_time.time);
|
||||
p_time+=boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6));
|
||||
d_position_UTC_time = p_time;
|
||||
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
|
||||
|
||||
DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time)
|
||||
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
|
||||
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
|
||||
|
||||
// ######## LOG FILE #########
|
||||
if(d_flag_dump_enabled == true)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double = Rx_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = rx_position_and_time(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = rx_position_and_time(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = rx_position_and_time(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = rx_position_and_time(3);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return b_valid_position;
|
||||
}
|
||||
return b_valid_position;
|
||||
}
|
||||
|
||||
@ -71,17 +71,21 @@
|
||||
class rtklib_solver : public Pvt_Solution
|
||||
{
|
||||
private:
|
||||
|
||||
rtk_t rtk_;
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
sol_t pvt_sol;
|
||||
bool d_flag_dump_enabled;
|
||||
int d_nchannels; // Number of available channels for positioning
|
||||
public:
|
||||
rtklib_solver(int nchannels,std::string dump_filename, bool flag_dump_to_file, rtk_t & rtk);
|
||||
rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t & rtk);
|
||||
~rtklib_solver();
|
||||
|
||||
bool get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, double Rx_time, bool flag_averaging);
|
||||
int d_nchannels; //!< Number of available channels for positioning
|
||||
bool get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_map, double Rx_time, bool flag_averaging);
|
||||
|
||||
std::map<int,Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
|
||||
std::map<int,Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new GPS_Ephemeris
|
||||
std::map<int,Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map;
|
||||
std::map<int,Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
|
||||
std::map<int,Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new GPS_Ephemeris
|
||||
std::map<int,Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map; //!< Map storing new GPS_CNAV_Ephemeris
|
||||
|
||||
Galileo_Utc_Model galileo_utc_model;
|
||||
Galileo_Iono galileo_iono;
|
||||
@ -94,14 +98,6 @@ public:
|
||||
Gps_CNAV_Utc_Model gps_cnav_utc_model;
|
||||
|
||||
int count_valid_position;
|
||||
|
||||
bool d_flag_dump_enabled;
|
||||
|
||||
sol_t pvt_sol;
|
||||
rtk_t rtk_;
|
||||
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
Loading…
Reference in New Issue
Block a user