Merge pull request #200 from swift-nav/work_saturdays

Fix end-of-week failures

include/libswiftnav/gpstime.h

@@ -24,23 +24,28 @@
 
 /** Offset between GPS and UTC times in seconds.
  * Update when a new leap second is inserted and be careful about times in the
- * past when this offset was different. */
+ * past when this offset was different.
+ * TODO handle leap seconds properly!
+ */
 #define GPS_MINUS_UTC_SECS 17
 
 /** Unix timestamp of the GPS epoch 1980-01-06 00:00:00 UTC */
 #define GPS_EPOCH 315964800
 
+#define WN_UNKNOWN -1
+
 /** Structure representing a GPS time. */
 typedef struct __attribute__((packed)) {
   double tow; /**< Seconds since the GPS start of week. */
-  u16 wn;     /**< GPS week number. */
+  s16 wn;     /**< GPS week number. */
 } gps_time_t;
 
 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/ephemeris.c

@@ -20,6 +20,9 @@
 #include "constants.h"
 #include "ephemeris.h"
 
+#define EPHEMERIS_VALID_TIME (4*60*60) /* seconds +/- from epoch.
+                                          TODO: should be 2 hrs? */
+
 /** Calculate satellite position, velocity and clock offset from ephemeris.
  *
  * References:
@@ -59,8 +62,8 @@ s8 calc_sat_state(const ephemeris_t *ephemeris, gps_time_t t,
   dt = gpsdifftime(t, ephemeris->toe);
 
   /* If dt is greater than 4 hours our ephemeris isn't valid. */
-  if (fabs(dt) > 4*3600) {
-    log_warn("Using ephemeris older (or newer!) than 4 hours.\n");
+  if (fabs(dt) > EPHEMERIS_VALID_TIME) {
+    log_error("Using ephemeris outside validity period, dt = %+.0f\n", dt);
     return -1;
   }
 
@@ -168,7 +171,7 @@ u8 ephemeris_good(ephemeris_t *eph, gps_time_t t)
 
   /* TODO: this doesn't exclude ephemerides older than a week so could be made
    * better. */
-  return (eph->valid && eph->healthy && fabs(dt) < 4*3600);
+  return (eph->valid && eph->healthy && fabs(dt) < EPHEMERIS_VALID_TIME);
 }
 
 /** Decode ephemeris from L1 C/A GPS navigation message frames.

src/gpstime.c

@@ -11,9 +11,13 @@
  */
 
 #include <math.h>
-
 #include "gpstime.h"
 
+#define WEEK_SECS (7*24*60*60)
+
+/* TODO: does it make sense to be passing structs by value in all
+   these functions? */
+
 /** Normalize a `gps_time_t` GPS time struct.
  * Ensures that the time of week is greater than zero and less than one week by
  * wrapping and adjusting the week number accordingly.
@@ -25,12 +29,12 @@
 gps_time_t normalize_gps_time(gps_time_t t)
 {
   while(t.tow < 0) {
-    t.tow += 3600*24*7;
+    t.tow += WEEK_SECS;
     t.wn += 1;
   }
 
-  while(t.tow > 3600*24*7) {
-    t.tow -= 3600*24*7;
+  while(t.tow > WEEK_SECS) {
+    t.tow -= WEEK_SECS;
     t.wn -= 1;
   }
 
@@ -47,13 +51,16 @@ time_t gps2time(gps_time_t gps_t)
 {
   time_t t = GPS_EPOCH - GPS_MINUS_UTC_SECS;
 
-  t += 7*24*3600*gps_t.wn;
+  t += WEEK_SECS*gps_t.wn;
   t += (s32)gps_t.tow;
 
   return t;
 }
 
 /** Time difference in seconds between two GPS times.
+ * If the week number field of either time is unspecified, the result
+ * will be as if the week numbers had been chosen for the times to be
+ * as close as possible.
  * \param end Higher bound of the time interval whose length is calculated.
  * \param beginning Lower bound of the time interval whose length is
  *                  calculated. If this describes a time point later than end,
@@ -62,8 +69,33 @@ time_t gps2time(gps_time_t gps_t)
  */
 double gpsdifftime(gps_time_t end, gps_time_t beginning)
 {
-  return (end.wn - beginning.wn)*7*24*3600 + \
-         end.tow - beginning.tow;
+  double dt = end.tow - beginning.tow;
+  if (end.wn == WN_UNKNOWN || beginning.wn == WN_UNKNOWN) {
+    /* One or both of the week numbers is unspecified.  Assume times
+       are within +/- 0.5 weeks of each other. */
+    if (dt > WEEK_SECS / 2)
+      dt -= WEEK_SECS;
+    if (dt < -WEEK_SECS / 2)
+      dt += WEEK_SECS;
+  } else {
+    /* Week numbers were provided - use them. */
+    dt += (end.wn - beginning.wn) * WEEK_SECS;
+  }
+  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;

tests/CMakeLists.txt

@@ -41,6 +41,7 @@ else (CMAKE_CROSSCOMPILING)
       check_filter_utils.c
       check_ephemeris.c
       check_set.c
+      check_gpstime.c
     )
 
     target_link_libraries(test_libswiftnav ${TEST_LIBS})

tests/check_gpstime.c

@@ -0,0 +1,42 @@
+#include <check.h>
+#include <stdio.h>
+#include <math.h>
+#include "check_utils.h"
+
+#include "gpstime.h"
+
+START_TEST(test_gpsdifftime)
+{
+  struct gpsdifftime_testcase {
+    gps_time_t a, b;
+    double dt;
+  } testcases[] = {
+    {.a = {567890.0, 1234}, .b = {567890.0, 1234}, .dt = 0},
+    {.a = {567890.0, 1234}, .b = {0.0, 1234}, .dt = 567890},
+    {.a = {567890.0, WN_UNKNOWN}, .b = {0.0, 1234}, .dt = -36910},
+    {.a = {222222.0, 2222}, .b = {2222.0, WN_UNKNOWN}, .dt = 220000},
+    {.a = {444444.0, WN_UNKNOWN}, .b = {2222.0, WN_UNKNOWN}, .dt = -162578},
+    {.a = {604578.0, 1000}, .b = {222.222, 1001}, .dt = -444.222},
+    {.a = {604578.0, 1001}, .b = {222.222, 1000}, .dt = 1209155.778},
+  };
+  const double tow_tol = 1e-10;
+  for (size_t i = 0;
+       i < sizeof(testcases) / sizeof(struct gpsdifftime_testcase); i++) {
+    double dt = gpsdifftime(testcases[i].a, testcases[i].b);
+    fail_unless(fabs(dt - testcases[i].dt) < tow_tol,
+                "gpsdifftime test case %d failed, dt = %.12f", i, dt);
+  }
+}
+END_TEST
+
+Suite* gpstime_test_suite(void)
+{
+  Suite *s = suite_create("GPS time handling");
+
+  TCase *tc_core = tcase_create("Core");
+  tcase_add_test(tc_core, test_gpsdifftime);
+  suite_add_tcase(s, tc_core);
+
+  return s;
+}
+

tests/check_main.c

@@ -26,6 +26,7 @@ int main(void)
   srunner_add_suite(sr, filter_utils_suite());
   srunner_add_suite(sr, ephemeris_suite());
   srunner_add_suite(sr, set_suite());
+  srunner_add_suite(sr, gpstime_test_suite());
 
   srunner_set_fork_status(sr, CK_NOFORK);
   srunner_run_all(sr, CK_NORMAL);

tests/check_suites.h

@@ -17,5 +17,6 @@ Suite* ambiguity_test_suite(void);
 Suite* filter_utils_suite(void);
 Suite* ephemeris_suite(void);
 Suite* set_suite(void);
+Suite* gpstime_test_suite(void);
 
 #endif /* CHECK_SUITES_H */