diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 5b6a9f584..526392885 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -47,7 +47,9 @@ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/ + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * -----------------------------------------------------------------------*/ #include "rtklib_solver.h" #include @@ -68,26 +70,25 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag d_flag_averaging = false; rtk_ = rtk; - pvt_sol = {{0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, - '0', '0', '0', 0, 0, 0 }; + pvt_sol = {{0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, '0', '0', '0', 0, 0, 0 }; // ############# 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(); + } + } } - } } @@ -96,6 +97,7 @@ rtklib_solver::~rtklib_solver() d_dump_file.close(); } + bool rtklib_solver::get_PVT(std::map gnss_observables_map, double Rx_time, bool flag_averaging) { std::map::iterator gnss_observables_iter; @@ -116,190 +118,194 @@ bool rtklib_solver::get_PVT(std::map gnss_observables_map, dou for(gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) - { - switch(gnss_observables_iter->second.System) { - case 'E': - { - // 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()) + switch(gnss_observables_iter->second.System) { - //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++; + case 'E': + { + // 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': + { + // 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; + } + } + 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[valid_obs] = insert_obs_to_rtklib(obs_data[valid_obs], + 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; } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - break; } - case 'G': - { - // 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; - } - } - 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(gnss_observables_iter->second.PRN)) - { - eph_data[i]=eph_to_rtklib(gps_cnav_ephemeris_iter->second); - obs_data[valid_obs]=insert_obs_to_rtklib(obs_data[valid_obs], - 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; - } - } // ********************************************************************** // ****** SOLVE PVT****************************************************** // ********************************************************************** 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++) + if (valid_obs > 0) { - 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; /* L2 */ - } - - result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data); - if(result==0) - { - LOG(INFO)<<"RTKLIB rtkpos error message: "<[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 + int result = 0; + nav_t nav_data; + nav_data.eph = eph_data; + nav_data.n = valid_obs; + for (int i = 0; i < MAXSAT; i++) { - 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)); + 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; /* L2 */ } - catch (const std::ifstream::failure& e) + + result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data); + if(result == 0) { - 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; } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 62a073263..5ab5ee5bb 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -47,7 +47,10 @@ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/ + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * -------------------------------------------------------------------------*/ + #ifndef GNSS_SDR_RTKLIB_SOLVER_H_ #define GNSS_SDR_RTKLIB_SOLVER_H_