- Port recent updates from demo5 RTKLIB code

- Add output of residuals file in same format as RTKLIB
- Make trace output more similar to demo5 RTKLIB code

Author: rtklibexplorer

src/pntpos.py

@@ -85,7 +85,7 @@ def rescode(iter, obs, nav, rs, dts, svh, x):
         r, e = geodist(rs[i], rr)
         if r < 0:
             continue
-        az, el = satazel(pos, e)
+        [az, el] = satazel(pos, e)
         if el < nav.elmin:
             continue
         if iter > 0:

src/postpos.py

@@ -80,13 +80,13 @@ def savesol(sol, solfile):
                     s.stat, s.ns, std[1,1], std[0,0], std[2,2], std[0,1],
                     std[2,0], std[1,2], s.age, s.ratio))
 
-def procpos(nav, rov, base):
+def procpos(nav, rov, base, fp_stat):
 
     try:
         if nav.filtertype != 'backward':
             # run forward solution
             firstpos(nav, rov, base, dir=1)
-            rtkpos(nav, rov, base, dir=1) 
+            rtkpos(nav, rov, base, fp_stat, dir=1) 
             sol0 = deepcopy(nav.sol)
             savesol(sol0,'forward.pos')
         if nav.filtertype != 'forward':
@@ -105,7 +105,7 @@ def procpos(nav, rov, base):
                 firstpos(nav, rov, base, dir=-1)
             else: # combined_noreset
                 nav.sol = [nav.sol[-1]]
-            rtkpos(nav, rov, base, dir=-1)  
+            rtkpos(nav, rov, base, fp_stat, dir=-1)  
             savesol(nav.sol,'backward.pos')
         if nav.filtertype == 'combined' or nav.filtertype == 'combined_noreset':
             sol = combres(sol0, nav.sol)

src/rinex.py

@@ -139,6 +139,9 @@ def decode_nav(self, navfile, nav):
                     eph.tot = gpst2time(eph.week, tot)
                     nav.eph.append(eph)
                 else:  # GLONASS
+                    if prn > uGNSS.GLOMAX:
+                        print('Reject nav entry: %s' % line[:3])
+                        break
                     geph = Geph(sat)
                     # Toc rounded by 15 min in utc 
                     week, tow = time2gpst(toc)
@@ -370,7 +373,7 @@ def next_obs(nav, rov, base, dir):
 def rcvstds(nav, obs):
     """ decode receiver stdevs from rinex fields """
     # skip if weighting factor is zero
-    if nav.err[3] == 0:
+    if nav.err[5] == 0:
         return
     for i in np.argsort(obs.sat):
         for f in range(nav.nf):

src/rtkcmn.py

@@ -237,8 +237,10 @@ def __init__(self, cfg):
         self.slip = np.zeros((uGNSS.MAXSAT, self.nf), dtype=int)
         self.prev_lli = np.zeros((uGNSS.MAXSAT, self.nf, 2), dtype=int)
         self.prev_fix = np.zeros((uGNSS.MAXSAT, self.nf), dtype=int)
-        self.rcvstd = np.zeros((uGNSS.MAXSAT, self.nf*2))
         self.glofrq = np.zeros(uGNSS.GLOMAX, dtype=int)
+        self.rcvstd = np.zeros((uGNSS.MAXSAT, self.nf*2))
+        self.resp = np.zeros((uGNSS.MAXSAT, self.nf))
+        self.resc = np.zeros((uGNSS.MAXSAT, self.nf))
 
         self.prev_ratio1 = 0
         self.prev_ratio2 = 0
@@ -597,17 +599,20 @@ def ecef2pos(r):
 def pos2ecef(pos, isdeg: bool = False):
     """ LLH (rad/deg) to ECEF position conversion  """
     if isdeg:
-        pos[0] *= np.pi/180.0
-        pos[1] *= np.pi/180.0
-    s_p = sin(pos[0])
-    c_p = cos(pos[0])
-    s_l = sin(pos[1])
-    c_l = cos(pos[1])
-    e2 = rCST.FE_WGS84 * (2.0 - rCST.FE_WGS84)
-    v = rCST.RE_WGS84 / sqrt(1.0 - e2 * s_p**2)
-    r = np.array([(v + pos[2]) * c_p*c_l,
-                  (v + pos[2]) * c_p*s_l,
-                  (v * (1.0 - e2) + pos[2]) * s_p])
+        s_p = sin(pos[0]*np.pi/180.0)
+        c_p = cos(pos[0]*np.pi/180.0)
+        s_l = sin(pos[1]*np.pi/180.0)
+        c_l = cos(pos[1]*np.pi/180.0)
+    else:
+        s_p = sin(pos[0])
+        c_p = cos(pos[0])
+        s_l = sin(pos[1])
+        c_l = cos(pos[1])
+        e2 = rCST.FE_WGS84 * (2.0 - rCST.FE_WGS84)
+        v = rCST.RE_WGS84 / sqrt(1.0 - e2 * s_p**2)
+        r = np.array([(v + pos[2]) * c_p*c_l,
+                      (v + pos[2]) * c_p*s_l,
+                      (v * (1.0 - e2) + pos[2]) * s_p])
     return r
 
 
@@ -617,6 +622,12 @@ def ecef2enu(pos, r):
     e = E @ r
     return e
 
+def enu2ecef(pos, e):
+    """ relative ECEF to ENU conversion """
+    E = xyz2enu(pos)
+    r = E.T @ e
+    return r
+
 def covenu(llh, P):
     """transform ecef covariance to local tangental coordinate --------------------------
     * transform ecef covariance to local tangental coordinate
@@ -659,9 +670,9 @@ def satazel(pos, e):
         az = atan2(enu[0], enu[1]) if np.dot(enu, enu) > 1e-12 else 0
         az = az if az > 0 else az + 2 * np.pi
         el = asin(enu[2])
-        return az, el
+        return [az, el]
     else:
-        return 0, np.pi / 2
+        return [0, np.pi / 2]
 
 
 def ionmodel(t, pos, az, el, ion=None):