gps: fix calculation of cnt in the past, PPS may be updated after packet is received and stamped5.0.21

2 files changed, 53 insertions, 9 deletions

diff --git a/libloragw/src/loragw_gps.c b/libloragw/src/loragw_gps.c
index d02e23a..b461439 100644
--- a/libloragw/src/loragw_gps.c
+++ b/libloragw/src/loragw_gps.c
@@ -720,12 +720,19 @@ int lgw_cnt2utc(struct tref ref, uint32_t count_us, struct timespec *utc) {
     }
 
     /* calculate delta in seconds between reference count_us and target count_us */
-    delta_sec = (double)(count_us - ref.count_us) / (TS_CPS * ref.xtal_err);
+    if (count_us > ref.count_us) {
+        delta_sec = (double)(count_us - ref.count_us) / (TS_CPS * ref.xtal_err);
+    } else {
+        delta_sec = -(double)(ref.count_us - count_us) / (TS_CPS * ref.xtal_err);
+    }
 
     /* now add that delta to reference UTC time */
     fractpart = modf (delta_sec , &intpart);
     tmp = ref.utc.tv_nsec + (long)(fractpart * 1E9);
-    if (tmp < (long)1E9) { /* the nanosecond part doesn't overflow */
+    if (tmp < 0) {
+        utc->tv_sec = ref.utc.tv_sec + (time_t)intpart - 1;
+        utc->tv_nsec = (long)1E9 + tmp;
+    } else if (tmp < (long)1E9) { /* the nanosecond part doesn't overflow */
         utc->tv_sec = ref.utc.tv_sec + (time_t)intpart;
         utc->tv_nsec = tmp;
     } else { /* must carry one second */
@@ -747,6 +754,11 @@ int lgw_utc2cnt(struct tref ref, struct timespec utc, uint32_t *count_us) {
         return LGW_GPS_ERROR;
     }
 
+    if (utc.tv_sec < ref.utc.tv_sec || (utc.tv_sec == ref.utc.tv_sec && utc.tv_nsec < ref.utc.tv_nsec)) {
+        DEBUG_MSG("ERROR: UTC time has passed\n");
+        return LGW_GPS_ERROR;
+    }
+
     /* calculate delta in seconds between reference utc and target utc */
     delta_sec = (double)(utc.tv_sec - ref.utc.tv_sec);
     delta_sec += 1E-9 * (double)(utc.tv_nsec - ref.utc.tv_nsec);
@@ -770,13 +782,20 @@ int lgw_cnt2gps(struct tref ref, uint32_t count_us, struct timespec *gps_time) {
         return LGW_GPS_ERROR;
     }
 
-    /* calculate delta in milliseconds between reference count_us and target count_us */
-    delta_sec = (double)(count_us - ref.count_us) / (TS_CPS * ref.xtal_err);
+    /* calculate delta in seconds between reference count_us and target count_us */
+    if (count_us > ref.count_us) {
+        delta_sec = (double)(count_us - ref.count_us) / (TS_CPS * ref.xtal_err);
+    } else {
+        delta_sec = -(double)(ref.count_us - count_us) / (TS_CPS * ref.xtal_err);
+    }
 
     /* now add that delta to reference GPS time */
     fractpart = modf (delta_sec , &intpart);
     tmp = ref.gps.tv_nsec + (long)(fractpart * 1E9);
-    if (tmp < (long)1E9) { /* the nanosecond part doesn't overflow */
+    if (tmp < 0) {
+        gps_time->tv_sec = ref.gps.tv_sec + (time_t)intpart - 1;
+        gps_time->tv_nsec = (long)1E9 + tmp;
+    } else if (tmp < (long)1E9) { /* the nanosecond part doesn't overflow */
         gps_time->tv_sec = ref.gps.tv_sec + (time_t)intpart;
         gps_time->tv_nsec = tmp;
     } else { /* must carry one second */
@@ -798,6 +817,11 @@ int lgw_gps2cnt(struct tref ref, struct timespec gps_time, uint32_t *count_us) {
         return LGW_GPS_ERROR;
     }
 
+    if (gps_time.tv_sec < ref.gps.tv_sec || (gps_time.tv_sec == ref.gps.tv_sec && gps_time.tv_nsec < ref.gps.tv_nsec)) {
+        DEBUG_MSG("ERROR: GPS time has passed\n");
+        return LGW_GPS_ERROR;
+    }
+
     /* calculate delta in seconds between reference gps time and target gps time */
     delta_sec = (double)(gps_time.tv_sec - ref.gps.tv_sec);
     delta_sec += 1E-9 * (double)(gps_time.tv_nsec - ref.gps.tv_nsec);
diff --git a/libloragw/tst/test_loragw_gps.c b/libloragw/tst/test_loragw_gps.c
index c535878..6d41a0d 100644
--- a/libloragw/tst/test_loragw_gps.c
+++ b/libloragw/tst/test_loragw_gps.c
@@ -112,16 +112,36 @@ static void gps_process_sync(void) {
     x = ppm_tstamp + 500000;
     printf("    * Test of timestamp counter <-> GPS value conversion *\n");
     printf("    Test value: %u\n", x);
-    lgw_cnt2gps(ppm_ref, x, &y);
+    i = lgw_cnt2gps(ppm_ref, x, &y);
     printf("    Conversion to GPS: %lld.%09ld\n", (long long)y.tv_sec, y.tv_nsec);
-    lgw_gps2cnt(ppm_ref, y, &z);
+    i = lgw_gps2cnt(ppm_ref, y, &z);
     printf("    Converted back: %u ==> %dµs\n", z, (int32_t)(z-x));
     printf("    * Test of timestamp counter <-> UTC value conversion *\n");
     printf("    Test value: %u\n", x);
-    lgw_cnt2utc(ppm_ref, x, &y);
+    i = lgw_cnt2utc(ppm_ref, x, &y);
     printf("    Conversion to UTC: %lld.%09ld\n", (long long)y.tv_sec, y.tv_nsec);
-    lgw_utc2cnt(ppm_ref, y, &z);
+    i = lgw_utc2cnt(ppm_ref, y, &z);
     printf("    Converted back: %u ==> %dµs\n", z, (int32_t)(z-x));
+
+    x = ppm_tstamp - 500000;
+    printf("    * Test of timestamp counter <-> past GPS value conversion *\n");
+    printf("    Test value: %u\n", x);
+    i = lgw_cnt2gps(ppm_ref, x, &y);
+    printf("    Conversion to GPS: %lld.%09ld\n", (long long)y.tv_sec, y.tv_nsec);
+    i = lgw_gps2cnt(ppm_ref, y, &z);
+    if (i != LGW_GPS_ERROR)
+        printf("    ** FAILED **: Converted past time back: %u ==> %dµs\n", z, (int32_t)(z-x));
+    else
+        printf("    ** PASS **: Could not convert to CNT, GPS time has passed\n");
+    printf("    * Test of timestamp counter <-> past UTC value conversion *\n");
+    printf("    Test value: %u\n", x);
+    i = lgw_cnt2utc(ppm_ref, x, &y);
+    printf("    Conversion to UTC: %lld.%09ld\n", (long long)y.tv_sec, y.tv_nsec);
+    i = lgw_utc2cnt(ppm_ref, y, &z);
+    if (i != LGW_GPS_ERROR)
+        printf("    ** FAILED **: Converted past time back: %u ==> %dµs\n", z, (int32_t)(z-x));
+    else
+        printf("    ** PASS **: Could not convert to CNT, UTC time has passed\n");
 }
 
 static void gps_process_coords(void) {