Fixed test gps utility

1 files changed, 30 insertions, 141 deletions

diff --git a/libloragw/tst/test_loragw_gps.c b/libloragw/tst/test_loragw_gps.c
index fd8c61b..bb84068 100644
--- a/libloragw/tst/test_loragw_gps.c
+++ b/libloragw/tst/test_loragw_gps.c
@@ -65,72 +65,6 @@ static void sig_handler(int sigio) {
     }
 }
 
-static void gps_process_sync(void) {
-    /* variables for PPM pulse GPS synchronization */
-    uint32_t ppm_tstamp;
-    struct timespec ppm_gps;
-    struct timespec ppm_utc;
-
-    /* variables for timestamp <-> GPS time conversions */
-    uint32_t x, z;
-    struct timespec y;
-
-    /* get GPS time for synchronization */
-    int i = lgw_gps_get(&ppm_utc, &ppm_gps, NULL, NULL);
-    if (i != LGW_GPS_SUCCESS) {
-        printf("    No valid reference GPS time available, synchronization impossible.\n");
-        return;
-    }
-
-    /* get timestamp for synchronization */
-    i = lgw_get_trigcnt(&ppm_tstamp);
-    if (i != LGW_HAL_SUCCESS) {
-        printf("    Failed to read timestamp, synchronization impossible.\n");
-        return;
-    }
-
-    /* try to update synchronize time reference with the new GPS & timestamp */
-    i = lgw_gps_sync(&ppm_ref, ppm_tstamp, ppm_utc, ppm_gps);
-    if (i != LGW_GPS_SUCCESS) {
-        printf("    Synchronization error.\n");
-        return;
-    }
-
-    /* display result */
-    printf("    * Synchronization successful *\n");
-    printf("    UTC reference time: %lld.%09ld\n", (long long)ppm_ref.utc.tv_sec, ppm_ref.utc.tv_nsec);
-    printf("    GPS reference time: %lld.%09ld\n", (long long)ppm_ref.gps.tv_sec, ppm_ref.gps.tv_nsec);
-    printf("    Internal counter reference value: %u\n", ppm_ref.count_us);
-    printf("    Clock error: %.9f\n", ppm_ref.xtal_err);
-
-    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);
-    printf("    Conversion to GPS: %lld.%09ld\n", (long long)y.tv_sec, y.tv_nsec);
-    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);
-    printf("    Conversion to UTC: %lld.%09ld\n", (long long)y.tv_sec, y.tv_nsec);
-    lgw_utc2cnt(ppm_ref, y, &z);
-    printf("    Converted back: %u ==> %dµs\n", z, (int32_t)(z-x));
-}
-
-static void gps_process_coords(void) {
-    /* position variable */
-    struct coord_s coord;
-    struct coord_s gpserr;
-    int    i = lgw_gps_get(NULL, NULL, &coord, &gpserr);
-
-    /* update gateway coordinates */
-    if (i == LGW_GPS_SUCCESS) {
-        printf("# GPS coordinates: latitude %.5f, longitude %.5f, altitude %i m\n", coord.lat, coord.lon, coord.alt);
-        printf("# GPS err:         latitude %.5f, longitude %.5f, altitude %i m\n", gpserr.lat, gpserr.lon, gpserr.alt);
-    }
-}
-
 /* -------------------------------------------------------------------------- */
 /* --- MAIN FUNCTION -------------------------------------------------------- */
 
@@ -149,6 +83,7 @@ int main()
 
     /* NMEA/UBX variables */
     enum gps_msg latest_msg = UNKNOWN; /* keep track of latest NMEA/UBX message parsed */
+    static struct tref time_reference_gps; /* time reference used for GPS <-> timestamp conversion */
 
     fd_set fds;
     char delim[4] = "$";
@@ -196,82 +131,36 @@ int main()
     memset(&ppm_ref, 0, sizeof ppm_ref);
 
     /* loop until user action */
+
     while ((quit_sig != 1) && (exit_sig != 1)) {
-        int r = 0;
-        struct timeval tv;
-        tv.tv_sec = 0;
-        tv.tv_usec = 100000;
-        FD_ZERO(&fds);
-        FD_SET(gpsdata.gps_fd, &fds);
-        errno = 0;
-        r = select(gpsdata.gps_fd+1, &fds, NULL, NULL, &tv);
-        if (r == -1 && errno != EINTR) {
-            printf("gpspipe: select error %s(%d)\n", strerror(errno), errno);
-            exit(EXIT_FAILURE);
-        } else if (r == 0)
-            continue;
-
-        /* reading directly from the socket avoids decode overhead */
-        errno = 0;
-        r = (int)read(gpsdata.gps_fd, serial_buff, sizeof(serial_buff));
-        if (r > 0) {
-            int i = 0;
-            size_t frame_size = 0;
-            for (i = 0; i < r; i++) {
-                if (serial_buff[i] == (char)LGW_GPS_UBX_SYNC_CHAR) {
-                    /***********************
-                     * Found UBX sync char *
-                     ***********************/
-                    size_t ubx_size = (uint8_t)serial_buff[i+4];
-                    ubx_size |= (uint8_t)serial_buff[i+5] << 8;
-                    ubx_size += 8;
-                    if (ubx_size < 27){
-                        latest_msg = lgw_parse_ubx(&serial_buff[i], ubx_size , &frame_size);
-                    }
-                    if (frame_size > 0) {
-                        if(latest_msg == INVALID || latest_msg == UNKNOWN) {
-                            /* checksum failed */
-                            frame_size = 0;
-                        } else if (latest_msg == UBX_NAV_TIMEGPS) {
-                            gps_process_sync();
-                        }
-                    }
-                } else if((serial_buff[i] == LGW_GPS_NMEA_SYNC_CHAR) && (serial_buff[i+1] == 0x47) && (serial_buff[i+2] == 0x50)){
-                    /************************
-                     * Found NMEA sync char *
-                     ************************/
-                    int k, l= 0;
-                    token[0] = strtok(serial_buff, delim);
-
-                    while (token[l] != NULL) {
-                        l++;
-                        token[l] = strtok(NULL, delim);
-                    }
-                    for (k=0; k<=l-1; k++) {
-                        if ((strlen(token[k]) > 66) && (strlen(token[k]) < 74)){
-                            lgw_parse_nmea(token[k], strlen(token[k]));
-                            if(latest_msg == INVALID || latest_msg == UNKNOWN) {
-                                /* checksum failed */
-                                frame_size = 0;
-                            } else if (latest_msg == NMEA_RMC) {
-                                /* Get location from RMC frames */
-                                gps_process_coords();
-                            }
-                        }
-                    }
-                }
-            }
-        } else {
-            if (r == -1) {
-                if (errno == EAGAIN)
-                    continue;
-                else {
-                    printf(stderr, "gpspipe: read error %s(%d)\n", strerror(errno), errno);
-                    exit(EXIT_FAILURE);
-                }
-            } else {
-                exit(EXIT_SUCCESS);
-            }
+        wait_ms(100);
+        int r = gps_read(&gpsdata, 0, 0);
+        int used=0;
+        if (r!= -1 && (gpsdata.status != STATUS_NO_FIX) &&
+                (gpsdata.fix.mode == MODE_2D || gpsdata.fix.mode == MODE_3D) &&
+                !isnan(gpsdata.fix.latitude) &&
+                !isnan(gpsdata.fix.longitude)) {
+
+
+            uint32_t trig_tstamp; /* concentrator timestamp associated with PPM pulse */
+
+            i = lgw_get_trigcnt(&trig_tstamp);
+            lgw_gps_sync(&time_reference_gps, trig_tstamp, gpsdata.fix.time);
+
+            printf("\n--- GPS ---\n");
+            printf("Set:             %d\n", gpsdata.set);
+            printf("Online:  %10.0f\n", gpsdata.online);
+            printf("Status:          %d\n", gpsdata.status);
+            printf("Satellites Used: %d\n", gpsdata.satellites_used);
+            printf("Mode:            %d\n", gpsdata.fix.mode);
+            printf("UTC time:        %lld.%09ld\n", (long long)time_reference_gps.utc.tv_sec, time_reference_gps.utc.tv_nsec);
+            printf("GPS time:        %lld.%09ld\n", (long long)time_reference_gps.gps.tv_sec, time_reference_gps.gps.tv_nsec);
+            printf("Latitude:        %f\n", gpsdata.fix.latitude);
+            printf("Longitude:       %f\n", gpsdata.fix.longitude);
+            printf("Altitude:        %fm\n", gpsdata.fix.altitude);
+            printf("Speed:           %f\n", gpsdata.fix.speed);
+            printf("Track:           %f\n", gpsdata.fix.track);
+            printf("Pdop:            %f\n", gpsdata.dop.pdop);
         }
     }