From 06140620f7a7dc2ed906ff1a0a7904ebe5f2435e Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 9 Oct 2018 12:33:38 +0200 Subject: [PATCH 1/5] Add RTKLIB fixes for Galileo --- src/algorithms/libs/rtklib/rtklib.h | 9 ++-- .../libs/rtklib/rtklib_ephemeris.cc | 45 +++++++++++++++++-- src/algorithms/libs/rtklib/rtklib_ephemeris.h | 1 + src/algorithms/libs/rtklib/rtklib_pntpos.cc | 14 ++++-- src/algorithms/libs/rtklib/rtklib_ppp.cc | 11 ++++- src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 17 +++++-- src/algorithms/libs/rtklib/rtklib_rtkcmn.h | 2 +- src/algorithms/libs/rtklib/rtklib_rtkpos.cc | 13 +++++- src/algorithms/libs/rtklib/rtklib_stream.cc | 4 +- 9 files changed, 95 insertions(+), 21 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index 14d90b902..0036a98a2 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -274,7 +274,7 @@ const int MAXRCV = 64; //!< max receiver number (1 to MAXRCV) const int MAXOBSTYPE = 64; //!< max number of obs type in RINEX const double MAXDTOE = 7200.0; //!< max time difference to GPS Toe (s) const double MAXDTOE_QZS = 7200.0; //!< max time difference to QZSS Toe (s) -const double MAXDTOE_GAL = 10800.0; //!< max time difference to Galileo Toe (s) +const double MAXDTOE_GAL = 14400.0; //!< max time difference to Galileo Toe (s) const double MAXDTOE_BDS = 21600.0; //!< max time difference to BeiDou Toe (s) const double MAXDTOE_GLO = 1800.0; //!< max time difference to GLONASS Toe (s) const double MAXDTOE_SBS = 360.0; //!< max time difference to SBAS Toe (s) @@ -299,6 +299,9 @@ const int IONOOPT_QZS = 6; //!< ionosphere option: QZSS broadcast model const int IONOOPT_LEX = 7; //!< ionosphere option: QZSS LEX ionospehre const int IONOOPT_STEC = 8; //!< ionosphere option: SLANT TEC model +const int GALMESS_INAV = 0; //!< Galileo message type: FNAV */ +const int GALMESS_FNAV = 1; //!< Galileo message type: INAV */ + const int TROPOPT_OFF = 0; //!< troposphere option: correction off const int TROPOPT_SAAS = 1; //!< troposphere option: Saastamoinen model const int TROPOPT_SBAS = 2; //!< troposphere option: SBAS model @@ -1210,7 +1213,7 @@ typedef struct char local[1024]; /* local file path */ int topts[4]; /* time options {poff,tint,toff,tretry} (s) */ gtime_t tnext; /* next retry time (gpst) */ - pthread_t thread; /* download thread */ + pthread_t thread; /* download thread */ } ftp_t; @@ -1283,7 +1286,7 @@ typedef struct stream_t stream[8]; /* streams {rov,base,corr,sol1,sol2,logr,logb,logc} */ stream_t *moni; /* monitor stream */ unsigned int tick; /* start tick */ - pthread_t thread; /* server thread */ + pthread_t thread; /* server thread */ int cputime; /* CPU time (ms) for a processing cycle */ int prcout; /* missing observation data count */ lock_t lock; /* lock flag */ diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index eff182fbd..b0ac91269 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -84,6 +84,7 @@ const double STD_BRDCCLK = 30.0; /* error of broadcast clock (m) const int MAX_ITER_KEPLER = 30; /* max number of iteration of Kelpler */ +int galmessagetype; /* variance by ura ephemeris (ref [1] 20.3.3.3.1.1) --------------------------*/ double var_uraeph(int ura) @@ -94,6 +95,15 @@ double var_uraeph(int ura) return ura < 0 || 14 < ura ? std::pow(6144.0, 2.0) : std::pow(ura_value[ura], 2.0); } +/* variance by sisa ephemeris (ref [7] Issue 1.3 5.1.11) --------------------------*/ +double var_sisaeph(int ura) +{ + if (ura < 50) return std::pow(ura * 0.01, 2.0); + if (ura < 75) return std::pow(0.50 + (ura - 50) * 0.02, 2.0); + if (ura < 100) return std::pow(1.0 + (ura - 75) * 0.04, 2.0); + if (ura <= 125) return std::pow(2.0 + (ura - 100) * 0.16, 2.0); + return -1.0; +} /* variance by ura ssr (ref [4]) ---------------------------------------------*/ double var_urassr(int ura) @@ -289,7 +299,14 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, *dts -= 2.0 * sqrt(mu * eph->A) * eph->e * sinE / std::pow(SPEED_OF_LIGHT, 2.0); /* position and clock error variance */ - *var = var_uraeph(eph->sva); + if (sys == SYS_GAL) + { + *var = var_sisaeph(eph->sva); + } + else + { + *var = var_uraeph(eph->sva); + } } @@ -454,16 +471,18 @@ eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav) { double t, tmax, tmin; int i, j = -1; + int sys; trace(4, "seleph : time=%s sat=%2d iode=%d\n", time_str(time, 3), sat, iode); - switch (satsys(sat, NULL)) + sys = satsys(sat, NULL); + switch (sys) { case SYS_QZS: tmax = MAXDTOE_QZS + 1.0; break; case SYS_GAL: - tmax = MAXDTOE_GAL + 1.0; + tmax = MAXDTOE_GAL; break; case SYS_BDS: tmax = MAXDTOE_BDS + 1.0; @@ -478,7 +497,25 @@ eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav) { if (nav->eph[i].sat != sat) continue; if (iode >= 0 && nav->eph[i].iode != iode) continue; - if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) continue; + if (sys == SYS_GAL) + { + if (galmessagetype == GALMESS_FNAV) + { + /* Check if F/NAV ephemeris */ + if (!(nav->eph[i].code & 0x02)) continue; + } + else + { + /* Check if I/NAV ephemeris */ + if (!(nav->eph[i].code & 0x05)) continue; + } + if ((t = timediff(time, nav->eph[i].toe)) > tmax) continue; + if (t < 0) continue; + } + else + { + if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) continue; + } if (iode >= 0) return nav->eph + i; if (t <= tmin) { diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.h b/src/algorithms/libs/rtklib/rtklib_ephemeris.h index b6eaf6b78..e36970cfd 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.h +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.h @@ -58,6 +58,7 @@ double var_uraeph(int ura); +double var_sisaeph(int ura); double var_urassr(int ura); void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts); double eph2clk(gtime_t time, const eph_t *eph); diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index d4a10fe65..b56265d28 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -402,7 +402,8 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, double *resp, int *ns) { double r, dion, dtrp, vmeas, vion, vtrp, rr[3], pos[3], dtr, e[3], P, lam_L1; - int i, j, nv = 0, sys, mask[4] = {0}; + int i, j, nv = 0, sys, mask[4] = {0}, sva = -1; + eph_t *eph; trace(3, "resprng : n=%d\n", n); @@ -438,15 +439,20 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, trace(4, "satazel error. el = %lf , elmin = %lf\n", elaux, opt->elmin); continue; } - /* psudorange with code bias correction */ + /* pseudorange with code bias correction */ if ((P = prange(obs + i, nav, azel + i * 2, iter, opt, &vmeas)) == 0.0) { trace(4, "prange error\n"); continue; } - + /* if Gal satellite, check SISA is not NAPA*/ + if (sys == SYS_GAL) + { + if (!(eph = seleph(obs[i].time, obs[i].sat, -1, nav))) continue; + sva = eph->sva; + } /* excluded satellite? */ - if (satexclude(obs[i].sat, svh[i], opt)) + if (satexclude(obs[i].sat, svh[i], sva, opt)) { trace(4, "satexclude error\n"); continue; diff --git a/src/algorithms/libs/rtklib/rtklib_ppp.cc b/src/algorithms/libs/rtklib/rtklib_ppp.cc index b60d627c9..3e8c93c81 100644 --- a/src/algorithms/libs/rtklib/rtklib_ppp.cc +++ b/src/algorithms/libs/rtklib/rtklib_ppp.cc @@ -1522,7 +1522,8 @@ int res_ppp(int iter __attribute__((unused)), const obsd_t *obs, int n, const do prcopt_t *opt = &rtk->opt; double r, rr[3], disp[3], pos[3], e[3], meas[2], dtdx[3], dantr[NFREQ] = {0}; double dants[NFREQ] = {0}, var[MAXOBS * 2], dtrp = 0.0, vart = 0.0, varm[2] = {0}; - int i, j, k, sat, sys, nv = 0, nx = rtk->nx, brk, tideopt; + int i, j, k, sat, sys, nv = 0, nx = rtk->nx, brk, tideopt, sva = -1; + eph_t *eph; trace(3, "res_ppp : n=%d nx=%d\n", n, nx); @@ -1550,8 +1551,14 @@ int res_ppp(int iter __attribute__((unused)), const obsd_t *obs, int n, const do if ((r = geodist(rs + i * 6, rr, e)) <= 0.0 || satazel(pos, e, azel + i * 2) < opt->elmin) continue; + /* if Gal satellite, check SISA is not NAPA*/ + if (sys == SYS_GAL) + { + if (!(eph = seleph(obs[i].time, obs[i].sat, -1, nav))) continue; + sva = eph->sva; + } /* excluded satellite? */ - if (satexclude(obs[i].sat, svh[i], opt)) continue; + if (satexclude(obs[i].sat, svh[i], sva, opt)) continue; /* tropospheric delay correction */ if (opt->tropopt == TROPOPT_SAAS) diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index f7e4e76c5..95a9c8aad 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -464,10 +464,11 @@ void satno2id(int sat, char *id) * test excluded satellite * args : int sat I satellite number * int svh I sv health flag + * int sva I SV URA/SISA * prcopt_t *opt I processing options (NULL: not used) * return : status (1:excluded,0:not excluded) *-----------------------------------------------------------------------------*/ -int satexclude(int sat, int svh, const prcopt_t *opt) +int satexclude(int sat, int svh, const int sva, const prcopt_t *opt) { int sys = satsys(sat, NULL); @@ -492,6 +493,13 @@ int satexclude(int sat, int svh, const prcopt_t *opt) } } if (sys == SYS_QZS) svh &= 0xFE; /* mask QZSS LEX health */ + + /* Exclude Galileo satellites with NAPA or invalid SISA */ + if ((sys == SYS_GAL) && (sva > 125)) + { + trace(3, "Galileo satellite with NAPA or invalid SISA: sat=%3d", sat); + return 1; + } if (svh) { trace(3, "unhealthy satellite: sat=%3d svh=%02X\n", sat, svh); @@ -2740,7 +2748,7 @@ int cmpeph(const void *p1, const void *p2) void uniqeph(nav_t *nav) { eph_t *nav_eph; - int i, j; + int i, j, sys; trace(3, "uniqeph: n=%d\n", nav->n); @@ -2750,8 +2758,11 @@ void uniqeph(nav_t *nav) for (i = 1, j = 0; i < nav->n; i++) { + sys = satsys(nav->eph[i].sat, NULL); if (nav->eph[i].sat != nav->eph[j].sat || - nav->eph[i].iode != nav->eph[j].iode) + nav->eph[i].iode != nav->eph[j].iode || + ((sys == SYS_GAL) && (nav->eph[i].code != nav->eph[j].code)) || + (nav->eph[i].toe.sec != nav->eph[j].toe.sec)) { nav->eph[++j] = nav->eph[i]; } diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h index f58e95193..1b3092b58 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h @@ -121,7 +121,7 @@ int satno(int sys, int prn); int satsys(int sat, int *prn); int satid2no(const char *id); void satno2id(int sat, char *id); -int satexclude(int sat, int svh, const prcopt_t *opt); +int satexclude(int sat, int svh, const int sva, const prcopt_t *opt); int testsnr(int base, int freq, double el, double snr, const snrmask_t *mask); unsigned char obs2code(const char *obs, int *freq); char *code2obs(unsigned char code, int *freq); diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index 80c1e0f0c..d75cd85ff 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -1097,7 +1097,8 @@ int zdres(int base, const obsd_t *obs, int n, const double *rs, { double r, rr_[3], pos[3], dant[NFREQ] = {0}, disp[3]; double zhd, zazel[] = {0.0, 90.0 * D2R}; - int i, nf = NF_RTK(opt); + int i, nf = NF_RTK(opt), sys, sva = -1; + eph_t *eph; trace(3, "zdres : n=%d\n", n); @@ -1122,8 +1123,16 @@ int zdres(int base, const obsd_t *obs, int n, const double *rs, if ((r = geodist(rs + i * 6, rr_, e + i * 3)) <= 0.0) continue; if (satazel(pos, e + i * 3, azel + i * 2) < opt->elmin) continue; + /* if Gal satellite, check SISA is not NAPA*/ + if (!(sys = satsys(obs[i].sat, NULL))) continue; + if (sys == SYS_GAL) + { + if (!(eph = seleph(obs[i].time, obs[i].sat, -1, nav))) continue; + sva = eph->sva; + } + /* excluded satellite? */ - if (satexclude(obs[i].sat, svh[i], opt)) continue; + if (satexclude(obs[i].sat, svh[i], sva, opt)) continue; /* satellite clock-bias */ r += -SPEED_OF_LIGHT * dts[i * 2]; diff --git a/src/algorithms/libs/rtklib/rtklib_stream.cc b/src/algorithms/libs/rtklib/rtklib_stream.cc index 04ae96e01..9fe57fe1e 100644 --- a/src/algorithms/libs/rtklib/rtklib_stream.cc +++ b/src/algorithms/libs/rtklib/rtklib_stream.cc @@ -96,7 +96,7 @@ serial_t *openserial(const char *path, int mode, char *msg) if (!(serial = (serial_t *)malloc(sizeof(serial_t)))) return NULL; - if ((p = strchr((char *)path, ':'))) + if ((p = const_cast(strchr((char *)path, ':')))) { strncpy(port, path, p - path); port[p - path] = '\0'; @@ -1589,7 +1589,7 @@ void *ftpthread(void *arg) /* proxy settings for wget (ref [2]) */ if (*proxyaddr) { - proto = ftp->proto ? (char *)"http" : (char *)"ftp"; + proto = const_cast(ftp->proto ? (char *)"http" : (char *)"ftp"); sprintf(env, "set %s_proxy=http://%s & ", proto, proxyaddr); proxyopt = (char *)"--proxy=on "; } From 120c1a06143b05a54a9f8d233ba6eade03653330 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 9 Oct 2018 17:00:43 +0200 Subject: [PATCH 2/5] Galileo fix --- src/algorithms/libs/rtklib/rtklib_ephemeris.cc | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index b0ac91269..a650de082 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -499,6 +499,15 @@ eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav) if (iode >= 0 && nav->eph[i].iode != iode) continue; if (sys == SYS_GAL) { + if (nav->eph[i].sat > (NSATGPS + NSATGLO)) + { + galmessagetype = GALMESS_INAV; + } + else + { + galmessagetype = GALMESS_FNAV; + } + if (galmessagetype == GALMESS_FNAV) { /* Check if F/NAV ephemeris */ From 19308ded3c186819183416e13df1c8a33767aa02 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 11 Oct 2018 14:12:29 +0200 Subject: [PATCH 3/5] Revert "Galileo fix" This reverts commit 120c1a06143b05a54a9f8d233ba6eade03653330. --- src/algorithms/libs/rtklib/rtklib_ephemeris.cc | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index a650de082..b0ac91269 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -499,15 +499,6 @@ eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav) if (iode >= 0 && nav->eph[i].iode != iode) continue; if (sys == SYS_GAL) { - if (nav->eph[i].sat > (NSATGPS + NSATGLO)) - { - galmessagetype = GALMESS_INAV; - } - else - { - galmessagetype = GALMESS_FNAV; - } - if (galmessagetype == GALMESS_FNAV) { /* Check if F/NAV ephemeris */ From 97f4aefb6e8bd56b9ad797c89dea1a11d4c3dde9 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 11 Oct 2018 14:13:03 +0200 Subject: [PATCH 4/5] Revert "Add RTKLIB fixes for Galileo" This reverts commit 06140620f7a7dc2ed906ff1a0a7904ebe5f2435e. --- src/algorithms/libs/rtklib/rtklib.h | 9 ++-- .../libs/rtklib/rtklib_ephemeris.cc | 45 ++----------------- src/algorithms/libs/rtklib/rtklib_ephemeris.h | 1 - src/algorithms/libs/rtklib/rtklib_pntpos.cc | 14 ++---- src/algorithms/libs/rtklib/rtklib_ppp.cc | 11 +---- src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 17 ++----- src/algorithms/libs/rtklib/rtklib_rtkcmn.h | 2 +- src/algorithms/libs/rtklib/rtklib_rtkpos.cc | 13 +----- src/algorithms/libs/rtklib/rtklib_stream.cc | 4 +- 9 files changed, 21 insertions(+), 95 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index 0036a98a2..14d90b902 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -274,7 +274,7 @@ const int MAXRCV = 64; //!< max receiver number (1 to MAXRCV) const int MAXOBSTYPE = 64; //!< max number of obs type in RINEX const double MAXDTOE = 7200.0; //!< max time difference to GPS Toe (s) const double MAXDTOE_QZS = 7200.0; //!< max time difference to QZSS Toe (s) -const double MAXDTOE_GAL = 14400.0; //!< max time difference to Galileo Toe (s) +const double MAXDTOE_GAL = 10800.0; //!< max time difference to Galileo Toe (s) const double MAXDTOE_BDS = 21600.0; //!< max time difference to BeiDou Toe (s) const double MAXDTOE_GLO = 1800.0; //!< max time difference to GLONASS Toe (s) const double MAXDTOE_SBS = 360.0; //!< max time difference to SBAS Toe (s) @@ -299,9 +299,6 @@ const int IONOOPT_QZS = 6; //!< ionosphere option: QZSS broadcast model const int IONOOPT_LEX = 7; //!< ionosphere option: QZSS LEX ionospehre const int IONOOPT_STEC = 8; //!< ionosphere option: SLANT TEC model -const int GALMESS_INAV = 0; //!< Galileo message type: FNAV */ -const int GALMESS_FNAV = 1; //!< Galileo message type: INAV */ - const int TROPOPT_OFF = 0; //!< troposphere option: correction off const int TROPOPT_SAAS = 1; //!< troposphere option: Saastamoinen model const int TROPOPT_SBAS = 2; //!< troposphere option: SBAS model @@ -1213,7 +1210,7 @@ typedef struct char local[1024]; /* local file path */ int topts[4]; /* time options {poff,tint,toff,tretry} (s) */ gtime_t tnext; /* next retry time (gpst) */ - pthread_t thread; /* download thread */ + pthread_t thread; /* download thread */ } ftp_t; @@ -1286,7 +1283,7 @@ typedef struct stream_t stream[8]; /* streams {rov,base,corr,sol1,sol2,logr,logb,logc} */ stream_t *moni; /* monitor stream */ unsigned int tick; /* start tick */ - pthread_t thread; /* server thread */ + pthread_t thread; /* server thread */ int cputime; /* CPU time (ms) for a processing cycle */ int prcout; /* missing observation data count */ lock_t lock; /* lock flag */ diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index b0ac91269..eff182fbd 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -84,7 +84,6 @@ const double STD_BRDCCLK = 30.0; /* error of broadcast clock (m) const int MAX_ITER_KEPLER = 30; /* max number of iteration of Kelpler */ -int galmessagetype; /* variance by ura ephemeris (ref [1] 20.3.3.3.1.1) --------------------------*/ double var_uraeph(int ura) @@ -95,15 +94,6 @@ double var_uraeph(int ura) return ura < 0 || 14 < ura ? std::pow(6144.0, 2.0) : std::pow(ura_value[ura], 2.0); } -/* variance by sisa ephemeris (ref [7] Issue 1.3 5.1.11) --------------------------*/ -double var_sisaeph(int ura) -{ - if (ura < 50) return std::pow(ura * 0.01, 2.0); - if (ura < 75) return std::pow(0.50 + (ura - 50) * 0.02, 2.0); - if (ura < 100) return std::pow(1.0 + (ura - 75) * 0.04, 2.0); - if (ura <= 125) return std::pow(2.0 + (ura - 100) * 0.16, 2.0); - return -1.0; -} /* variance by ura ssr (ref [4]) ---------------------------------------------*/ double var_urassr(int ura) @@ -299,14 +289,7 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, *dts -= 2.0 * sqrt(mu * eph->A) * eph->e * sinE / std::pow(SPEED_OF_LIGHT, 2.0); /* position and clock error variance */ - if (sys == SYS_GAL) - { - *var = var_sisaeph(eph->sva); - } - else - { - *var = var_uraeph(eph->sva); - } + *var = var_uraeph(eph->sva); } @@ -471,18 +454,16 @@ eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav) { double t, tmax, tmin; int i, j = -1; - int sys; trace(4, "seleph : time=%s sat=%2d iode=%d\n", time_str(time, 3), sat, iode); - sys = satsys(sat, NULL); - switch (sys) + switch (satsys(sat, NULL)) { case SYS_QZS: tmax = MAXDTOE_QZS + 1.0; break; case SYS_GAL: - tmax = MAXDTOE_GAL; + tmax = MAXDTOE_GAL + 1.0; break; case SYS_BDS: tmax = MAXDTOE_BDS + 1.0; @@ -497,25 +478,7 @@ eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav) { if (nav->eph[i].sat != sat) continue; if (iode >= 0 && nav->eph[i].iode != iode) continue; - if (sys == SYS_GAL) - { - if (galmessagetype == GALMESS_FNAV) - { - /* Check if F/NAV ephemeris */ - if (!(nav->eph[i].code & 0x02)) continue; - } - else - { - /* Check if I/NAV ephemeris */ - if (!(nav->eph[i].code & 0x05)) continue; - } - if ((t = timediff(time, nav->eph[i].toe)) > tmax) continue; - if (t < 0) continue; - } - else - { - if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) continue; - } + if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) continue; if (iode >= 0) return nav->eph + i; if (t <= tmin) { diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.h b/src/algorithms/libs/rtklib/rtklib_ephemeris.h index e36970cfd..b6eaf6b78 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.h +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.h @@ -58,7 +58,6 @@ double var_uraeph(int ura); -double var_sisaeph(int ura); double var_urassr(int ura); void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts); double eph2clk(gtime_t time, const eph_t *eph); diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index b56265d28..d4a10fe65 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -402,8 +402,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, double *resp, int *ns) { double r, dion, dtrp, vmeas, vion, vtrp, rr[3], pos[3], dtr, e[3], P, lam_L1; - int i, j, nv = 0, sys, mask[4] = {0}, sva = -1; - eph_t *eph; + int i, j, nv = 0, sys, mask[4] = {0}; trace(3, "resprng : n=%d\n", n); @@ -439,20 +438,15 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, trace(4, "satazel error. el = %lf , elmin = %lf\n", elaux, opt->elmin); continue; } - /* pseudorange with code bias correction */ + /* psudorange with code bias correction */ if ((P = prange(obs + i, nav, azel + i * 2, iter, opt, &vmeas)) == 0.0) { trace(4, "prange error\n"); continue; } - /* if Gal satellite, check SISA is not NAPA*/ - if (sys == SYS_GAL) - { - if (!(eph = seleph(obs[i].time, obs[i].sat, -1, nav))) continue; - sva = eph->sva; - } + /* excluded satellite? */ - if (satexclude(obs[i].sat, svh[i], sva, opt)) + if (satexclude(obs[i].sat, svh[i], opt)) { trace(4, "satexclude error\n"); continue; diff --git a/src/algorithms/libs/rtklib/rtklib_ppp.cc b/src/algorithms/libs/rtklib/rtklib_ppp.cc index 1d565db11..7ac193643 100644 --- a/src/algorithms/libs/rtklib/rtklib_ppp.cc +++ b/src/algorithms/libs/rtklib/rtklib_ppp.cc @@ -1251,8 +1251,7 @@ int res_ppp(int iter __attribute__((unused)), const obsd_t *obs, int n, const do prcopt_t *opt = &rtk->opt; double r, rr[3], disp[3], pos[3], e[3], meas[2], dtdx[3], dantr[NFREQ] = {0}; double dants[NFREQ] = {0}, var[MAXOBS * 2], dtrp = 0.0, vart = 0.0, varm[2] = {0}; - int i, j, k, sat, sys, nv = 0, nx = rtk->nx, brk, tideopt, sva = -1; - eph_t *eph; + int i, j, k, sat, sys, nv = 0, nx = rtk->nx, brk, tideopt; trace(3, "res_ppp : n=%d nx=%d\n", n, nx); @@ -1280,14 +1279,8 @@ int res_ppp(int iter __attribute__((unused)), const obsd_t *obs, int n, const do if ((r = geodist(rs + i * 6, rr, e)) <= 0.0 || satazel(pos, e, azel + i * 2) < opt->elmin) continue; - /* if Gal satellite, check SISA is not NAPA*/ - if (sys == SYS_GAL) - { - if (!(eph = seleph(obs[i].time, obs[i].sat, -1, nav))) continue; - sva = eph->sva; - } /* excluded satellite? */ - if (satexclude(obs[i].sat, svh[i], sva, opt)) continue; + if (satexclude(obs[i].sat, svh[i], opt)) continue; /* tropospheric delay correction */ if (opt->tropopt == TROPOPT_SAAS) diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 95a9c8aad..f7e4e76c5 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -464,11 +464,10 @@ void satno2id(int sat, char *id) * test excluded satellite * args : int sat I satellite number * int svh I sv health flag - * int sva I SV URA/SISA * prcopt_t *opt I processing options (NULL: not used) * return : status (1:excluded,0:not excluded) *-----------------------------------------------------------------------------*/ -int satexclude(int sat, int svh, const int sva, const prcopt_t *opt) +int satexclude(int sat, int svh, const prcopt_t *opt) { int sys = satsys(sat, NULL); @@ -493,13 +492,6 @@ int satexclude(int sat, int svh, const int sva, const prcopt_t *opt) } } if (sys == SYS_QZS) svh &= 0xFE; /* mask QZSS LEX health */ - - /* Exclude Galileo satellites with NAPA or invalid SISA */ - if ((sys == SYS_GAL) && (sva > 125)) - { - trace(3, "Galileo satellite with NAPA or invalid SISA: sat=%3d", sat); - return 1; - } if (svh) { trace(3, "unhealthy satellite: sat=%3d svh=%02X\n", sat, svh); @@ -2748,7 +2740,7 @@ int cmpeph(const void *p1, const void *p2) void uniqeph(nav_t *nav) { eph_t *nav_eph; - int i, j, sys; + int i, j; trace(3, "uniqeph: n=%d\n", nav->n); @@ -2758,11 +2750,8 @@ void uniqeph(nav_t *nav) for (i = 1, j = 0; i < nav->n; i++) { - sys = satsys(nav->eph[i].sat, NULL); if (nav->eph[i].sat != nav->eph[j].sat || - nav->eph[i].iode != nav->eph[j].iode || - ((sys == SYS_GAL) && (nav->eph[i].code != nav->eph[j].code)) || - (nav->eph[i].toe.sec != nav->eph[j].toe.sec)) + nav->eph[i].iode != nav->eph[j].iode) { nav->eph[++j] = nav->eph[i]; } diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h index 1b3092b58..f58e95193 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h @@ -121,7 +121,7 @@ int satno(int sys, int prn); int satsys(int sat, int *prn); int satid2no(const char *id); void satno2id(int sat, char *id); -int satexclude(int sat, int svh, const int sva, const prcopt_t *opt); +int satexclude(int sat, int svh, const prcopt_t *opt); int testsnr(int base, int freq, double el, double snr, const snrmask_t *mask); unsigned char obs2code(const char *obs, int *freq); char *code2obs(unsigned char code, int *freq); diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index d75cd85ff..80c1e0f0c 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -1097,8 +1097,7 @@ int zdres(int base, const obsd_t *obs, int n, const double *rs, { double r, rr_[3], pos[3], dant[NFREQ] = {0}, disp[3]; double zhd, zazel[] = {0.0, 90.0 * D2R}; - int i, nf = NF_RTK(opt), sys, sva = -1; - eph_t *eph; + int i, nf = NF_RTK(opt); trace(3, "zdres : n=%d\n", n); @@ -1123,16 +1122,8 @@ int zdres(int base, const obsd_t *obs, int n, const double *rs, if ((r = geodist(rs + i * 6, rr_, e + i * 3)) <= 0.0) continue; if (satazel(pos, e + i * 3, azel + i * 2) < opt->elmin) continue; - /* if Gal satellite, check SISA is not NAPA*/ - if (!(sys = satsys(obs[i].sat, NULL))) continue; - if (sys == SYS_GAL) - { - if (!(eph = seleph(obs[i].time, obs[i].sat, -1, nav))) continue; - sva = eph->sva; - } - /* excluded satellite? */ - if (satexclude(obs[i].sat, svh[i], sva, opt)) continue; + if (satexclude(obs[i].sat, svh[i], opt)) continue; /* satellite clock-bias */ r += -SPEED_OF_LIGHT * dts[i * 2]; diff --git a/src/algorithms/libs/rtklib/rtklib_stream.cc b/src/algorithms/libs/rtklib/rtklib_stream.cc index 9fe57fe1e..04ae96e01 100644 --- a/src/algorithms/libs/rtklib/rtklib_stream.cc +++ b/src/algorithms/libs/rtklib/rtklib_stream.cc @@ -96,7 +96,7 @@ serial_t *openserial(const char *path, int mode, char *msg) if (!(serial = (serial_t *)malloc(sizeof(serial_t)))) return NULL; - if ((p = const_cast(strchr((char *)path, ':')))) + if ((p = strchr((char *)path, ':'))) { strncpy(port, path, p - path); port[p - path] = '\0'; @@ -1589,7 +1589,7 @@ void *ftpthread(void *arg) /* proxy settings for wget (ref [2]) */ if (*proxyaddr) { - proto = const_cast(ftp->proto ? (char *)"http" : (char *)"ftp"); + proto = ftp->proto ? (char *)"http" : (char *)"ftp"; sprintf(env, "set %s_proxy=http://%s & ", proto, proxyaddr); proxyopt = (char *)"--proxy=on "; } From 31ebf2a3119aafe1baede166ff478776de331927 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 16 Oct 2018 17:25:42 +0200 Subject: [PATCH 5/5] Add utility for assisted GNSS --- src/utils/CMakeLists.txt | 4 + src/utils/assist/CMakeLists.txt | 56 ++++++++ src/utils/assist/main.cc | 226 ++++++++++++++++++++++++++++++++ 3 files changed, 286 insertions(+) create mode 100644 src/utils/assist/CMakeLists.txt create mode 100644 src/utils/assist/main.cc diff --git a/src/utils/CMakeLists.txt b/src/utils/CMakeLists.txt index 1396ed3e6..7ac99ed7c 100644 --- a/src/utils/CMakeLists.txt +++ b/src/utils/CMakeLists.txt @@ -17,3 +17,7 @@ # add_subdirectory(front-end-cal) + +if(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA) + add_subdirectory(assist) +endif(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA) diff --git a/src/utils/assist/CMakeLists.txt b/src/utils/assist/CMakeLists.txt new file mode 100644 index 000000000..8bf5fb3a6 --- /dev/null +++ b/src/utils/assist/CMakeLists.txt @@ -0,0 +1,56 @@ +# Copyright (C) 2012-2018 (see AUTHORS file for a list of contributors) +# +# This file is part of GNSS-SDR. +# +# GNSS-SDR is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# GNSS-SDR is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with GNSS-SDR. If not, see . +# + +find_package(GPSTK QUIET) +if(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK) + set(GPSTK_LIBRARY ${CMAKE_CURRENT_SOURCE_DIR}/../../../thirdparty/gpstk-${GNSSSDR_GPSTK_LOCAL_VERSION}/install/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gpstk${CMAKE_SHARED_LIBRARY_SUFFIX} ) + set(GPSTK_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../thirdparty/gpstk-${GNSSSDR_GPSTK_LOCAL_VERSION}/install/include ) +endif(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK) + +set(CMAKE_INCLUDE_PATH ${CMAKE_INCLUDE_PATH} ${GPSTK_INCLUDE_DIR}/gpstk) + +include_directories( + ${CMAKE_SOURCE_DIR}/src/core/system_parameters + ${GFlags_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${GPSTK_INCLUDE_DIR}/gpstk + ${GPSTK_INCLUDE_DIR} +) + +add_executable(rinex2assist ${CMAKE_CURRENT_SOURCE_DIR}/main.cc) + +target_link_libraries(rinex2assist + ${Boost_LIBRARIES} + ${GPSTK_LIBRARY} + ${GFlags_LIBS} + gnss_sp_libs + gnss_rx) + +if(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK) + add_dependencies(rinex2assist gpstk-${GNSSSDR_GPSTK_LOCAL_VERSION}) +endif(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK) + + +add_custom_command(TARGET rinex2assist POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy $ + ${CMAKE_SOURCE_DIR}/install/$) + +install(TARGETS rinex2assist + RUNTIME DESTINATION bin + COMPONENT "rinex2assist" +) diff --git a/src/utils/assist/main.cc b/src/utils/assist/main.cc new file mode 100644 index 000000000..ebcc473d2 --- /dev/null +++ b/src/utils/assist/main.cc @@ -0,0 +1,226 @@ +/*! + * \file main.cc + * \brief converts navigation RINEX files into XML files for Assisted GNSS. + * \author Carles Fernandez-Prades, 2018. cfernandez(at)cttc.cat + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +//#include "gnss_sdr_flags.h" +#include "gps_ephemeris.h" +#include "galileo_ephemeris.h" +#include +#include +#include +#include +#include +#include + +int main(int argc, char** argv) +{ + const std::string intro_help( + std::string("\n rinex2assist converts navigation RINEX files into XML files for Assisted GNSS\n") + + "Copyright (C) 2018 (see AUTHORS file for a list of contributors)\n" + + "This program comes with ABSOLUTELY NO WARRANTY;\n" + + "See COPYING file to see a copy of the General Public License.\n \n" + + "Usage: \n" + + " rinex2assist []"); + + google::SetUsageMessage(intro_help); + google::SetVersionString("1.0"); + google::ParseCommandLineFlags(&argc, &argv, true); + + if ((argc < 2) or (argc > 3)) + { + std::cerr << "Usage:" << std::endl; + std::cerr << " " << argv[0] + << " []" + << std::endl; + google::ShutDownCommandLineFlags(); + return 1; + } + std::string xml_filename; + if (argc == 3) + { + xml_filename = argv[2]; + } + //std::string filename_rinex_nav = FLAGS_filename_rinex_nav; + std::map eph_map; + std::map eph_gal_map; + + int i = 0; + int j = 0; + try + { + // Read nav file + gpstk::Rinex3NavStream rnffs(argv[1]); // Open navigation data file + gpstk::Rinex3NavData rne; + gpstk::Rinex3NavHeader hdr; + + // read header + rnffs >> hdr; + + // Check that it really is a RINEX navigation file + if (hdr.fileType.substr(0, 1).compare("N") != 0) + { + std::cout << "This is not a valid RINEX navigation file" << std::endl; + return 1; + } + + while (rnffs >> rne) + { + if (rne.satSys.compare("G") == 0 or rne.satSys.empty()) + { + // Fill ephemeris object + Gps_Ephemeris eph; + eph.i_satellite_PRN = rne.PRNID; + eph.d_TOW = rne.xmitTime; + eph.d_IODE_SF2 = rne.IODE; + eph.d_IODE_SF3 = rne.IODE; + eph.d_Crs = rne.Crs; + eph.d_Delta_n = rne.dn; + eph.d_M_0 = rne.M0; + eph.d_Cuc = rne.Cuc; + eph.d_e_eccentricity = rne.ecc; + eph.d_Cus = rne.Cus; + eph.d_sqrt_A = rne.Ahalf; + eph.d_Toe = rne.Toe; + eph.d_Toc = rne.Toc; + eph.d_Cic = rne.Cic; + eph.d_OMEGA0 = rne.OMEGA0; + eph.d_Cis = rne.Cis; + eph.d_i_0 = rne.i0; + eph.d_Crc = rne.Crc; + eph.d_OMEGA = rne.w; + eph.d_OMEGA_DOT = rne.OMEGAdot; + eph.d_IDOT = rne.idot; + eph.i_code_on_L2 = rne.codeflgs; //!< If 1, P code ON in L2; if 2, C/A code ON in L2; + eph.i_GPS_week = rne.weeknum; + eph.b_L2_P_data_flag = rne.L2Pdata; + eph.i_SV_accuracy = rne.accuracy; + eph.i_SV_health = rne.health; + eph.d_TGD = rne.Tgd; + eph.d_IODC = rne.IODC; + eph.i_AODO = 0; // + eph.b_fit_interval_flag = (rne.fitint > 4) ? 1 : 0; + eph.d_spare1 = 0.0; + eph.d_spare2 = 0.0; + eph.d_A_f0 = rne.af0; + eph.d_A_f1 = rne.af1; + eph.d_A_f2 = rne.af2; + eph.b_integrity_status_flag = 0; // + eph.b_alert_flag = 0; // + eph.b_antispoofing_flag = 0; // + eph_map[i] = eph; + i++; + } + if (rne.satSys.compare("E") == 0) + { + // Read Galileo ephemeris + Galileo_Ephemeris eph; + eph.i_satellite_PRN = rne.PRNID; + eph.M0_1 = rne.M0; + eph.e_1 = rne.ecc; + eph.A_1 = rne.Ahalf; + eph.OMEGA_0_2 = rne.OMEGA0; + eph.i_0_2 = rne.i0; + eph.omega_2 = rne.w; + eph.OMEGA_dot_3 = rne.OMEGAdot; + eph.iDot_2 = rne.idot; + eph.C_uc_3 = rne.Cuc; + eph.C_us_3 = rne.Cus; + eph.C_rc_3 = rne.Crc; + eph.C_rs_3 = rne.Crs; + eph.C_ic_4 = rne.Cic; + eph.C_is_4 = rne.Cis; + eph.t0e_1 = rne.Toe; + eph.t0c_4 = rne.Toc; + eph.af0_4 = rne.af0; + eph.af1_4 = rne.af1; + eph.af2_4 = rne.af2; + eph_gal_map[j] = eph; + j++; + } + } + } + catch (std::exception& e) + { + std::cout << "Error reading the RINEX file: " << e.what() << std::endl; + google::ShutDownCommandLineFlags(); + return 1; + } + + if (i == 0 and j == 0) + { + std::cout << "No data found in the RINEX file. No XML file will be created." << std::endl; + google::ShutDownCommandLineFlags(); + return 1; + } + + // Write XML + if (i != 0) + { + std::ofstream ofs; + if (xml_filename.empty()) + { + xml_filename = "eph_GPS_L1CA.xml"; + } + try + { + ofs.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", eph_map); + } + catch (std::exception& e) + { + std::cout << "Problem creating the XML file: " << e.what() << std::endl; + google::ShutDownCommandLineFlags(); + return 1; + } + } + if (j != 0) + { + std::ofstream ofs2; + if (xml_filename.empty()) + { + xml_filename = "eph_Galileo_E1.xml"; + } + try + { + ofs2.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs2); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", eph_gal_map); + } + catch (std::exception& e) + { + std::cout << "Problem creating the XML file: " << e.what() << std::endl; + google::ShutDownCommandLineFlags(); + return 1; + } + } + google::ShutDownCommandLineFlags(); + return 0; +}