Merge pull request #345 from swift-nav/i344-calc_nav_meas

Add calc_sat_state error code handling

Author: valeri-atamaniouk

include/libswiftnav/track.h

@@ -232,9 +232,9 @@ void cn0_est_init(cn0_est_state_t *s, float bw, float cn0_0,
                   float cutoff_freq, float loop_freq);
 float cn0_est(cn0_est_state_t *s, float I, float Q);
 
-void calc_navigation_measurement(u8 n_channels, const channel_measurement_t *meas[],
-                                 navigation_measurement_t *nav_meas[],
-                                 double nav_time, const ephemeris_t* e[]);
+s8 calc_navigation_measurement(u8 n_channels, const channel_measurement_t *meas[],
+                               navigation_measurement_t *nav_meas[],
+                               double nav_time, const ephemeris_t* e[]);
 
 int nav_meas_cmp(const void *a, const void *b);
 u8 tdcp_doppler(u8 n_new, navigation_measurement_t *m_new,

python/tests/test_observation.py

@@ -165,7 +165,7 @@ def test_prop_single_diff():
                                 'toc': {'wn': 1876, 'tow': 172800.0}, 'dn': 4.718410826464475e-09, 'ecc': 0.0049016030970960855,
                                 'cic': -6.332993507385254e-08, 'crs': 19.0625, 'iode': 74, 'iodc': 21845, 'cis': -6.51925802230835e-08,
                                 'crc': 259.9375, 'w': 0.4885959643259506, 'af0': 7.212162017822266e-06, 'af1': 9.094947017729282e-13, 'af2': 0.0},
-                     'fit_interval': 4}),
+                     'fit_interval': 14400}),
         Ephemeris(**{'ura': 0.0, 'healthy': 1,
                      'xyz': {'acc': [-6.146728992462158e-08, 4.742340394655613e-09, -0.8114190126645531], 'iod': 0,
                              'a_gf0': 5153.798839569092, 'a_gf1': 1.6390180338742641,
@@ -180,7 +180,7 @@ def test_prop_single_diff():
                                 'toc': {'wn': 1876, 'tow': 172800.0}, 'dn': 4.742340394655613e-09, 'ecc': 0.00035172002390027046,
                                 'cic': 3.725290298461914e-09, 'crs': -40.5625, 'iode': 59, 'iodc': 21845, 'cis': -6.146728992462158e-08,
                                 'crc': 221.625, 'w': 2.9037284161724037, 'af0': -9.969808161258698e-07, 'af1': -5.229594535194337e-12, 'af2': 0.0},
-                     'fit_interval': 4})]
+                     'fit_interval': 14400})]
   gpst = ti.GpsTime(wn=1876, tow=167132)
   sdiffs = o.make_propagated_sdiffs_(rover_nm, rover_nm, remote_dists, base_pos, es, gpst)
   assert len(sdiffs) == 2

src/ephemeris.c

@@ -356,7 +356,11 @@ s8 calc_sat_state(const ephemeris_t *e, const gps_time_t *t,
 
   if (!ephemeris_valid(e, t)) {
     log_error_sid(e->sid,
-                  "Using invalid or too old ephemeris in calc_sat_state");
+                  "Using invalid or too old ephemeris in calc_sat_state"
+                  " (v:%d, fi:%d, [%d, %f]), [%d, %f]",
+                  (int)e->valid, (int)e->fit_interval,
+                  (int)e->toe.wn, e->toe.tow,
+                  (int)t->wn, t->tow);
     return -1;
   }
 

src/observation.c

@@ -193,8 +193,10 @@ u8 make_propagated_sdiffs(u8 n_local, navigation_measurement_t *m_local,
       double clock_rate_err;
       double local_sat_pos[3];
       double local_sat_vel[3];
-      calc_sat_state(e[i], t, local_sat_pos, local_sat_vel,
-                     &clock_err, &clock_rate_err);
+      if (calc_sat_state(e[i], t, local_sat_pos, local_sat_vel,
+                     &clock_err, &clock_rate_err) != 0) {
+        continue;
+      }
       sds[n].sid = m_local[i].sid;
       double dx = local_sat_pos[0] - remote_pos_ecef[0];
       double dy = local_sat_pos[1] - remote_pos_ecef[1];

src/track.c

@@ -760,9 +760,13 @@ float cn0_est(cn0_est_state_t *s, float I, float Q)
   return s->log_bw - 10.f*log10f(s->nsr);
 }
 
-void calc_navigation_measurement(u8 n_channels, const channel_measurement_t *meas[],
-                                 navigation_measurement_t *nav_meas[],
-                                 double nav_time, const ephemeris_t* e[])
+/**
+ * \return  0 on success,
+ *         -1 if ephemeris is invalid
+ */
+s8 calc_navigation_measurement(u8 n_channels, const channel_measurement_t *meas[],
+                               navigation_measurement_t *nav_meas[],
+                               double nav_time, const ephemeris_t* e[])
 {
   double TOTs[n_channels];
   double min_TOF = -DBL_MAX;
@@ -788,9 +792,11 @@ void calc_navigation_measurement(u8 n_channels, const channel_measurement_t *mea
     nav_meas[i]->lock_counter = meas[i]->lock_counter;
 
     /* calc sat clock error */
-    calc_sat_state(e[i], &nav_meas[i]->tot,
-                   nav_meas[i]->sat_pos, nav_meas[i]->sat_vel,
-                   &clock_err[i], &clock_rate_err[i]);
+    if (calc_sat_state(e[i], &nav_meas[i]->tot,
+                       nav_meas[i]->sat_pos, nav_meas[i]->sat_vel,
+                       &clock_err[i], &clock_rate_err[i]) != 0) {
+      return -1;
+    }
 
     /* remove clock error to put all tots within the same time window */
     if ((TOTs[i] + clock_err[i]) > min_TOF)
@@ -807,6 +813,8 @@ void calc_navigation_measurement(u8 n_channels, const channel_measurement_t *mea
     nav_meas[i]->tot.tow -= clock_err[i];
     normalize_gps_time(&nav_meas[i]->tot);
   }
+
+  return 0;
 }
 
 /** Compare navigation message by PRN.