Add RTKLIB fixes for Galileo

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

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,8 +299,15 @@ 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);
         }
+}
 
 
 /* glonass orbit differential equations --------------------------------------*/
@@ -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 (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)
                 {

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);

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;

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)

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];
                 }

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);

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];

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<char *>(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<char *>(ftp->proto ? (char *)"http" : (char *)"ftp");
             sprintf(env, "set %s_proxy=http://%s & ", proto, proxyaddr);
             proxyopt = (char *)"--proxy=on ";
         }