From 6c19437520d25f4c8656eb11904293a630775617 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 16 Aug 2017 01:01:59 +0200 Subject: [PATCH] Pass map of observables by reference --- src/algorithms/PVT/libs/rtklib_solver.cc | 473 ++++++++++++----------- src/algorithms/PVT/libs/rtklib_solver.h | 26 +- 2 files changed, 252 insertions(+), 247 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 053b8a926..f64be535c 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -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 gnss_observables_map, double Rx_time, bool flag_averaging) +bool rtklib_solver::get_PVT(const std::map & gnss_observables_map, double Rx_time, bool flag_averaging) { - std::map::iterator gnss_observables_iter; + std::map::const_iterator gnss_observables_iter; std::map::iterator galileo_ephemeris_iter; std::map::iterator gps_ephemeris_iter; std::map::iterator gps_cnav_ephemeris_iter; @@ -115,246 +125,245 @@ bool rtklib_solver::get_PVT(std::map 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(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(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(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(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(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(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(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(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; +} diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 5ab5ee5bb..6f441f4a6 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -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 gnss_observables_map, double Rx_time, bool flag_averaging); - int d_nchannels; //!< Number of available channels for positioning + bool get_PVT(const std::map & gnss_observables_map, double Rx_time, bool flag_averaging); - std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris - std::map gps_ephemeris_map; //!< Map storing new GPS_Ephemeris - std::map gps_cnav_ephemeris_map; + std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris + std::map gps_ephemeris_map; //!< Map storing new GPS_Ephemeris + std::map 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