From e947cd7d1c48d09b4b6d45776617f02170094021 Mon Sep 17 00:00:00 2001
From: Henry Hallam <henry@pericynthion.org>
Date: Tue, 21 Jul 2015 00:46:44 -0700
Subject: [PATCH] TRACK: Correctly match week number of measurement to
 ephemeris

---
 include/libswiftnav/gpstime.h |  1 +
 src/gpstime.c                 | 16 ++++++++++++++++
 src/track.c                   |  8 +++-----
 3 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/include/libswiftnav/gpstime.h b/include/libswiftnav/gpstime.h
index 31ed39d4..fb10640e 100644
--- a/include/libswiftnav/gpstime.h
+++ b/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 */
 
diff --git a/src/gpstime.c b/src/gpstime.c
index 4d70792b..e8d2bfb2 100644
--- a/src/gpstime.c
+++ b/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++;
+}
diff --git a/src/track.c b/src/track.c
index 868083f6..25a401a7 100644
--- a/src/track.c
+++ b/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;