diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 437acd35c..f0d593d4e 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -149,7 +149,11 @@ bool rtklib_solver::get_PVT(std::map gnss_observables_map, dou //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 - obs_data[valid_obs]=obs_to_rtklib(gnss_observables_iter->second, galileo_ephemeris_iter->second.WN_5); + obsd_t newobs={}; + 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 @@ -170,7 +174,11 @@ bool rtklib_solver::get_PVT(std::map gnss_observables_map, dou //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 - obs_data[valid_obs]=obs_to_rtklib(gnss_observables_iter->second, gps_ephemeris_iter->second.i_GPS_week); + obsd_t newobs={}; + 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 @@ -183,11 +191,37 @@ bool rtklib_solver::get_PVT(std::map gnss_observables_map, dou gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end()) { - //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 - obs_data[valid_obs]=obs_to_rtklib(gnss_observables_iter->second, gps_cnav_ephemeris_iter->second.i_GPS_week); - valid_obs++; + // 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;isecond.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) + std::cout<<"L2 observation attached!"<second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs={}; + 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 { diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 96350a2c6..0fce2e31b 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -52,20 +52,18 @@ #include "rtklib_conversions.h" -obsd_t obs_to_rtklib(Gnss_Synchro gnss_synchro, int week) +obsd_t insert_obs_to_rtklib(obsd_t rtklib_obs, Gnss_Synchro gnss_synchro, int week, int band) { - obsd_t rtklib_obs = {}; - rtklib_obs.D[0] = gnss_synchro.Carrier_Doppler_hz; - rtklib_obs.P[0] = gnss_synchro.Pseudorange_m; - rtklib_obs.L[0] = gnss_synchro.Carrier_phase_rads;//todo: check units - //rtklib_obs.SNR = gnss_synchro.CN0_dB_hz; - rtklib_obs.sat = gnss_synchro.PRN; - rtklib_obs.time = gpst2time(adjgpsweek(week), gnss_synchro.RX_time); + rtklib_obs.D[band]=gnss_synchro.Carrier_Doppler_hz; + rtklib_obs.P[band]=gnss_synchro.Pseudorange_m; + rtklib_obs.L[band]=gnss_synchro.Carrier_phase_rads;//todo: check units + //rtklib_obs.SNR=gnss_synchro.CN0_dB_hz; + rtklib_obs.sat=gnss_synchro.PRN; + rtklib_obs.time=gpst2time(adjgpsweek(week),gnss_synchro.RX_time); //printf("OBS RX TIME [%i]: %s,%f\n\r",rtklib_obs.sat,time_str(rtklib_obs.time,3),rtklib_obs.time.sec); return rtklib_obs; } - eph_t eph_to_rtklib(Galileo_Ephemeris gal_eph) { eph_t rtklib_sat = {}; diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.h b/src/algorithms/libs/rtklib/rtklib_conversions.h index 617557a62..67d8cd2d8 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.h +++ b/src/algorithms/libs/rtklib/rtklib_conversions.h @@ -62,6 +62,6 @@ eph_t eph_to_rtklib(Galileo_Ephemeris gal_eph); eph_t eph_to_rtklib(Gps_Ephemeris gps_eph); eph_t eph_to_rtklib(Gps_CNAV_Ephemeris gps_cnav_eph); -obsd_t obs_to_rtklib(Gnss_Synchro gnss_synchro, int week); +obsd_t insert_obs_to_rtklib(obsd_t rtklib_obs, Gnss_Synchro gnss_synchro, int week, int band); #endif /* GNSS_SDR_RTKLIB_CONVERSIONS_H_ */