TRACK: Correctly match week number of measurement to ephemeris

Author: henryhallam

include/libswiftnav/gpstime.h

@@ -45,6 +45,7 @@ gps_time_t normalize_gps_time(gps_time_t);
 time_t gps2time(gps_time_t t);
 
 double gpsdifftime(gps_time_t end, gps_time_t beginning);
+void gps_time_match_weeks(gps_time_t *t, const gps_time_t *ref);
 
 #endif /* LIBSWIFTNAV_TIME_H */
 

src/gpstime.c

@@ -83,3 +83,19 @@ double gpsdifftime(gps_time_t end, gps_time_t beginning)
   }
   return dt;
 }
+
+/** Set the week number of t so as to minimize the magnitude of the
+ * time difference between t and ref.
+ *
+ * \param t Pointer to GPS time whose week number will be set
+ * \param ref Reference GPS time
+ */
+void gps_time_match_weeks(gps_time_t *t, const gps_time_t *ref)
+{
+  t->wn = ref->wn;
+  double dt = t->tow - ref->tow;
+  if (dt > WEEK_SECS / 2)
+    t->wn--;
+  else if (dt < -WEEK_SECS / 2)
+    t->wn++;
+}

src/track.c

@@ -767,12 +767,10 @@ void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[],
     TOTs[i] += meas[i]->code_phase_chips / 1.023e6;
     TOTs[i] += (nav_time - meas[i]->receiver_time) * meas[i]->code_phase_rate / 1.023e6;
 
-    /** \todo Handle GPS time properly here, e.g. week rollover */
-    nav_meas[i]->tot.wn = ephemerides[i]->toe.wn;
+    /** \todo Maybe keep track of week number in tracking channel
+        state or derive it from system time. */
     nav_meas[i]->tot.tow = TOTs[i];
-
-    if (gpsdifftime(nav_meas[i]->tot, ephemerides[i]->toe) > 3*24*3600)
-      nav_meas[i]->tot.wn -= 1;
+    gps_time_match_weeks(&nav_meas[i]->tot, &ephemerides[i]->toe);
 
     nav_meas[i]->raw_doppler = meas[i]->carrier_freq;
     nav_meas[i]->snr = meas[i]->snr;