From 93640ffd465f8a03b6c32f404f88614453a73a38 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Wed, 11 Oct 2017 08:22:45 -0600 Subject: [PATCH] GLONASS bugfix: Fixes settings with rtklib obs_t and nav_t members Adds settings in rtklib code to parse GLONASS measurements in `obs_t` and 'nav_t' structures. It also adds the time of day field in gnav eph to keep track as of when ephemeris are reported. --- src/algorithms/PVT/libs/rtklib_solver.cc | 142 +++++++++--------- src/algorithms/libs/rtklib/rtklib.h | 7 + .../libs/rtklib/rtklib_conversions.cc | 13 +- src/algorithms/libs/rtklib/rtklib_rtksvr.cc | 8 +- .../glonass_gnav_ephemeris.h | 1 + .../glonass_gnav_navigation_message.cc | 3 + 6 files changed, 97 insertions(+), 77 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 612922dde..de34231d7 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -115,7 +115,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ std::map::const_iterator galileo_ephemeris_iter; std::map::const_iterator gps_ephemeris_iter; std::map::const_iterator gps_cnav_ephemeris_iter; - std::map::const_iterator glonass_gnav_ephemeris_iter; + std::map::const_iterator glonass_gnav_ephemeris_iter; this->set_averaging_flag(flag_averaging); @@ -123,6 +123,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ // ****** PREPARE THE DATA (SV EPHEMERIS AND OBSERVATIONS) ************************ // ******************************************************************************** int valid_obs = 0; //valid observations counter + int glo_valid_obs = 0; //GLONASS L1/L2 valid observations counter obsd_t obs_data[MAXOBS]; eph_t eph_data[MAXOBS]; @@ -278,76 +279,75 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ } break; } - case 'R': //TODO This should be using rtk lib nomenclature - { - std::string sig_(gnss_observables_iter->second.Signal); - // GLONASS GNAV L1 - if(sig_.compare("1G") == 0) - { - // 1 Glo - find the ephemeris for the current GLONASS SV observation. The SV Slot Number (PRN ID) is the map key - glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.end()) - { - //convert ephemeris from GNSS-SDR class to RTKLIB structure - geph_data[valid_obs] = eph_to_rtklib(glonass_gnav_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, - glonass_gnav_ephemeris_iter->second.d_WN, - 0);//TODO are THESE VALUES OK - valid_obs++; - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } + case 'R': //TODO This should be using rtk lib nomenclature + { + std::string sig_(gnss_observables_iter->second.Signal); + // GLONASS GNAV L1 + if(sig_.compare("1G") == 0) + { + // 1 Glo - find the ephemeris for the current GLONASS SV observation. The SV Slot Number (PRN ID) is the map key + glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend()) + { + //convert ephemeris from GNSS-SDR class to RTKLIB structure + geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + glonass_gnav_ephemeris_iter->second.d_WN, + 0);//Band 0 (L1) + glo_valid_obs++; + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } - } - // GLONASS GNAV L2 - if(sig_.compare("2G") == 0) - { - // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key - glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.end()) - { - bool found_L1_obs=false; - for (int i = 0; i < valid_obs; i++) - { - // TODO what is this? - if (geph_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, - glonass_gnav_ephemeris_iter->second.d_WN, - 2);//Band 3 (L5/E5) - found_L1_obs=true; - break; - } - } - if (!found_L1_obs) - { - //insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris - //convert ephemeris from GNSS-SDR class to RTKLIB structure - geph_data[valid_obs] = eph_to_rtklib(glonass_gnav_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; - } + } + // GLONASS GNAV L2 + if(sig_.compare("2G") == 0) + { + // 1 GLONASS - find the ephemeris for the current GLONASS SV observation. The SV PRN ID is the map key + glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend()) + { + bool found_L1_obs=false; + for (int i = 0; i < glo_valid_obs; i++) + { + if (geph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN+NSATGPS))) + { + obs_data[i] = insert_obs_to_rtklib(obs_data[i], + gnss_observables_iter->second, + glonass_gnav_ephemeris_iter->second.d_WN, + 1);//Band 1 (L2) + found_L1_obs=true; + break; + } + } + if (!found_L1_obs) + { + //insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris + //convert ephemeris from GNSS-SDR class to RTKLIB structure + geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + glonass_gnav_ephemeris_iter->second.d_WN, + 1); //Band 1 (L2) + glo_valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } - } - break; - } + } + break; + } default : DLOG(INFO) << "Hybrid observables: Unknown GNSS"; break; @@ -359,13 +359,15 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ // ********************************************************************** this->set_valid_position(false); - if (valid_obs > 0) + if (valid_obs > 0 || glo_valid_obs > 0) { int result = 0; nav_t nav_data; nav_data.eph = eph_data; nav_data.geph = geph_data; nav_data.n = valid_obs; + nav_data.ng = glo_valid_obs; + for (int i = 0; i < MAXSAT; i++) { nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1; /* L1/E1 */ @@ -373,7 +375,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; /* L5/E5 */ } - result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data); + result = rtkpos(&rtk_, obs_data, valid_obs + glo_valid_obs, &nav_data); if(result == 0) { LOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf; diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index ec0274b71..7aa7ddf5f 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -182,6 +182,7 @@ const int SYS_ALL = 0xFF; //!< navigation system: all +#define ENAGLO #ifdef ENAGLO const int MINPRNGLO = 1; //!< min satellite slot number of GLONASS const int MAXPRNGLO = 27; //!< max satellite slot number of GLONASS @@ -194,6 +195,12 @@ const int NSATGLO = 0; const int NSYSGLO = 0; #endif +/* +const int MINPRNGLO = 1; //!< min satellite slot number of GLONASS +const int MAXPRNGLO = 27; //!< max satellite slot number of GLONASS +const int NSATGLO = (MAXPRNGLO - MINPRNGLO + 1); //!< number of GLONASS satellites +const int NSYSGLO = 1; +*/ const int MINPRNGAL = 1; //!< min satellite PRN number of Galileo const int MAXPRNGAL = 30; //!< max satellite PRN number of Galileo const int NSATGAL = (MAXPRNGAL - MINPRNGAL + 1); //!< number of Galileo satellites diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 53d7bbe28..8b1c8c726 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -52,7 +52,7 @@ obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synch rtklib_obs.sat = gnss_synchro.PRN+NSATGPS+NSATGLO; break; case 'R': - rtklib_obs.sat = gnss_synchro.PRN; + rtklib_obs.sat = gnss_synchro.PRN+NSATGPS; break; default: @@ -66,12 +66,13 @@ obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synch geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { + int week; geph_t rtklib_sat = {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, 0.0}, 0.0, 0.0, 0.0}; gtime_t t_utc; struct tm utcinfo; - rtklib_sat.sat = glonass_gnav_eph.i_satellite_slot_number; /* satellite number */ + rtklib_sat.sat = glonass_gnav_eph.i_satellite_slot_number + NSATGPS; /* satellite number */ rtklib_sat.iode = static_cast(glonass_gnav_eph.d_t_b); /* IODE (0-6 bit of tb field) */ rtklib_sat.frq = glonass_gnav_eph.i_satellite_freq_channel; /* satellite frequency number */ rtklib_sat.svh = glonass_gnav_eph.d_l3rd_n; /* satellite health*/ @@ -93,17 +94,17 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) utcinfo.tm_mon = 0; utcinfo.tm_mday = glonass_gnav_eph.d_N_T; utcinfo.tm_year = glonass_gnav_eph.d_yr - 1900; - utcinfo.tm_hour = 6; // Diff between utc and (utc(su) + 3.00h) + utcinfo.tm_hour = -6; utcinfo.tm_min = 0; - utcinfo.tm_sec = glonass_gnav_eph.d_t_b; + utcinfo.tm_sec = glonass_gnav_eph.d_tod; t_utc.time = mktime(&utcinfo); t_utc.sec = glonass_gnav_eph.d_tau_c; - rtklib_sat.toe = utc2gpst(t_utc); /* epoch of epherides (gpst) */ + rtklib_sat.toe = utc2gpst(t_utc); /* message frame time (gpst) */ utcinfo.tm_mon = 0; utcinfo.tm_mday = glonass_gnav_eph.d_N_T; utcinfo.tm_year = glonass_gnav_eph.d_yr - 1900; - utcinfo.tm_hour = 6; + utcinfo.tm_hour = -6; utcinfo.tm_min = 0; utcinfo.tm_sec = glonass_gnav_eph.d_t_k; t_utc.time = mktime(&utcinfo); diff --git a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc index 0c2aabedf..b8c0288db 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc @@ -350,6 +350,12 @@ int decoderaw(rtksvr_t *svr, int index) /* decode download file ------------------------------------------------------*/ void decodefile(rtksvr_t *svr, int index) { + int i = 0; + char glo_fcn[MAXPRNGLO+1]; + + // Allocate space for GLONASS frequency channels depending on availability + for(i=0; i < MAXPRNGLO+1; i++) + glo_fcn[i]='0'; pcv_t pcvt0[MAXSAT] = { {0, {'0'}, {'0'}, {0, 0.0}, {0, 0.0}, {{0.0},{0.0}}, {{0.0},{0.0}} } }; sbsfcorr_t sbsfcorr0 = {{0, 0.0}, 0.0, 0.0, 0.0, 0, 0, 0}; sbslcorr_t sbslcorr0 = { {0, 0.0}, 0, {0.0}, {0.0}, 0.0, 0.0}; @@ -366,7 +372,7 @@ void decodefile(rtksvr_t *svr, int index) nav_t nav = {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, (erpd_t *){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, {{0.0},{0.0}}, {{0.0},{0.0}}, {{0.0},{0.0},{0.0}}, - {0.0}, {0.0}, '0', {*pcvt0}, sbssat0, {*sbsion0}, {*dgps0}, {*ssr0}, {*lexeph0}, + {0.0}, {0.0}, {*glo_fcn}, {*pcvt0}, sbssat0, {*sbsion0}, {*dgps0}, {*ssr0}, {*lexeph0}, {{0,0.0}, 0.0, {0.0}, {{0.0},{0.0}} }, pppcorr0} ; char file[1024]; diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index 281237b48..46ece9f95 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -102,6 +102,7 @@ public: double d_tau_c; double d_TOW; // tow of the start of frame double d_WN; // week number of the start of frame + double d_tod; template diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 1f3b5291d..7a9ea4ea8 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -521,6 +521,9 @@ int Glonass_Gnav_Navigation_Message::string_decoder(std::string frame_string) flag_TOW_new = true; } + // 4) Set time of day (tod) when ephemeris data is complety decoded + gnav_ephemeris.d_tod = gnav_ephemeris.d_t_k + 2*d_string_ID; + }