Changed gps functions to use gpspipe5.0.12

3 files changed, 280 insertions, 113 deletions

diff --git a/libloragw/inc/loragw_gps.h b/libloragw/inc/loragw_gps.h
index 0714974..3accae2 100644
--- a/libloragw/inc/loragw_gps.h
+++ b/libloragw/inc/loragw_gps.h
@@ -30,6 +30,7 @@ Maintainer: Michael Coracin
 #include <gpsd.h>
 #include <gpsdclient.h>
 #include <errno.h>      /* error messages */
+#include <sys/stat.h>
 
 #include "config.h"     /* library configuration options (dynamically generated) */
 /* -------------------------------------------------------------------------- */
@@ -102,19 +103,21 @@ enum gps_msg {
 /**
 @brief Configure a GPS module
 
-@param gpsdata handler for gpsd data
-@param source source for setup of gpsd
-@return success if the function was able to connect and configure a GPSD stream
+@param tty_path path to the TTY connected to the GPS
+@param gps_familly parameter (eg. ubx6 for uBlox gen.6)
+@param target_brate target baudrate for communication (0 keeps default target baudrate)
+@param fd_ptr pointer to a variable to receive file descriptor on GPS tty
+@return success if the function was able to connect and configure a GPS module
 */
-int lgw_gps_enable(struct gps_data_t *gpsdata, struct fixsource_t *source);
+int lgw_gps_enable(char* tty_path, char* gps_familly, speed_t target_brate, int* fd_ptr, int slot);
 
 /**
 @brief Restore GPS serial configuration and close serial device
 
-@param gpsdata handler for gpsd data
+@param fd file descriptor on GPS tty
 @return success if the function was able to complete
 */
-int lgw_gps_disable(struct gps_data_t *gpsdata);
+int lgw_gps_disable(int fd);
 
 /**
 @brief Get updated leap seconds
@@ -180,7 +183,7 @@ int lgw_gps_get(struct timespec *utc, struct timespec *gps_time, struct coord_s
 
 Set systime to 0 in ref to trigger initial synchronization.
 */
-int lgw_gps_sync(struct tref *ref, uint32_t count_us, struct timespec gps_time);
+int lgw_gps_sync(struct tref *ref, uint32_t count_us, struct timespec utc, struct timespec gps_time);
 
 /**
 @brief Convert concentrator timestamp counter value to UTC time
diff --git a/libloragw/src/loragw_gps.c b/libloragw/src/loragw_gps.c
index 2698c4b..43ec0bc 100644
--- a/libloragw/src/loragw_gps.c
+++ b/libloragw/src/loragw_gps.c
@@ -59,6 +59,20 @@ Maintainer: Michael Coracin
 
 #define UBX_MSG_NAVTIMEGPS_LEN  16
 
+char* fifo_name;
+FILE* pipe_fd;
+
+#define FIFO_1_NAME "/dev/gps1.fifo"
+#define FIFO_2_NAME "/dev/gps2.fifo"
+
+char fifo_1_make[] = "mkfifo " FIFO_1_NAME;
+char fifo_1_pipe[] = "gpspipe -R -r -o " FIFO_1_NAME;
+char fifo_1_kill[] = "pkill -f \"gpspipe.*" FIFO_1_NAME "\"";
+
+char fifo_2_make[] = "mkfifo " FIFO_2_NAME;
+char fifo_2_pipe[] = "gpspipe -R -r -o " FIFO_2_NAME;
+char fifo_2_kill[] = "pkill -f \"gpspipe.*" FIFO_2_NAME "\"";
+
 /* -------------------------------------------------------------------------- */
 /* --- PRIVATE VARIABLES ---------------------------------------------------- */
 
@@ -253,26 +267,40 @@ int str_chop(char *s, int buff_size, char separator, int *idx_ary, int max_idx)
 /* -------------------------------------------------------------------------- */
 /* --- PUBLIC FUNCTIONS DEFINITION ------------------------------------------ */
 
-int lgw_gps_enable(struct gps_data_t *gpsdata, struct fixsource_t *source) {
+int lgw_gps_enable(char *tty_path, char *gps_family, speed_t target_brate, int *fd_ptr, int slot) {
+    int i;
+    int gps_tty_dev; /* file descriptor to the serial port of the GNSS module */
 
-    unsigned int flags = WATCH_ENABLE| WATCH_JSON;
-    gpsd_source_spec(NULL, source);
+    /* check input parameters */
+    CHECK_NULL(tty_path);
+    CHECK_NULL(fd_ptr);
 
-    /* Update leap seconds from gpspipe */
-    if (lgw_get_leap_seconds() != LGW_GPS_SUCCESS) {
-        DEBUG_MSG("WARNING: Could not update leap seconds, using default value: %d\n", leap_seconds);
+    struct stat buf;
+
+    if (slot == 1) {
+        if (stat(FIFO_1_NAME, &buf) != 0) {
+            system(fifo_1_make);
+        }
+        system(fifo_1_kill);
+        pipe_fd = popen(fifo_1_pipe, "r");
+        fifo_name = FIFO_1_NAME;
     } else {
-        DEBUG_MSG("Updated leap seconds to: %d\n", leap_seconds);
+        if (stat(FIFO_2_NAME, &buf) != 0) {
+            system(fifo_2_make);
+        }
+        system(fifo_2_kill);
+        pipe_fd = popen(fifo_2_pipe, "r");
+        fifo_name = FIFO_2_NAME;
     }
 
+    /* open gps fifo */
+    gps_tty_dev = open(fifo_name, O_RDONLY );
 
-    if (gps_open(source->server, source->port, gpsdata) != 0) {
-        DEBUG_MSG("gpspipe: could not connect to gpsd %s:%s, %s(%d)\n",
-              source->server, source->port, gps_errstr(errno), errno);
+    if (gps_tty_dev <= 0) {
+        printf("ERROR: TTY PORT FAIL TO OPEN, CHECK PATH AND ACCESS RIGHTS\n");
         return LGW_GPS_ERROR;
     }
-
-    (void)gps_stream(gpsdata, flags, source->device);
+    *fd_ptr = gps_tty_dev;
 
     /* get timezone info */
     tzset();
@@ -287,15 +315,19 @@ int lgw_gps_enable(struct gps_data_t *gpsdata, struct fixsource_t *source) {
 
 /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
-int lgw_gps_disable(struct gps_data_t *gpsdata) {
+int lgw_gps_disable(int fd) {
     int i;
 
-    (void) gps_stream(&gps_data, WATCH_DISABLE, NULL);
+    i = close(fd);
+    i = pclose(pipe_fd);
+
+    if (memcmp(fifo_name, FIFO_1_NAME, 14) == 0)
+        system("rm /dev/gps1.fifo");
+    else
+        system("rm /dev/gps2.fifo");
 
-    /* ends the session */
-    i = gps_close(gpsdata);
     if (i != 0) {
-        DEBUG_MSG("ERROR: GPSD FAILED TO CLOSE\n");
+        DEBUG_MSG("ERROR: TTY PORT FAIL TO CLOSE - %s\n", strerror(errno));
         return LGW_GPS_ERROR;
     }
 
@@ -405,8 +437,7 @@ enum gps_msg lgw_parse_ubx(const char *serial_buff, size_t buff_size, size_t *ms
                         gps_week =  (uint8_t)serial_buff[14];
                         gps_week |= (uint8_t)serial_buff[15] << 8; /* GPS week number */
 
-
-
+                        gps_time_ok = true;
 #if 0
                         /* For debug */
                         {
@@ -421,23 +452,11 @@ enum gps_msg lgw_parse_ubx(const char *serial_buff, size_t buff_size, size_t *ms
                             printf("  GPS time = %02d:%02d:%02d\n", ubx_gps_hou, ubx_gps_min, ubx_gps_sec);
                         }
 #endif
-                        if (gps_lock_ok)
-                           gps_time_ok = true;
-
-                        return UBX_NAV_TIMEGPS;
-                    } else {
-                        gps_time_ok = false;
-                        return INVALID;
-            }
-                } else if ((serial_buff[2] == 0x01) && (serial_buff[3] == 0x04)) {
-                    if (serial_buff[10] == 0x0F && serial_buff[11] == 0x27
-                       && serial_buff[10] == 0x0F && serial_buff[11] == 0x27
-                       && serial_buff[10] == 0x0F && serial_buff[11] == 0x27
-                       && serial_buff[10] == 0x0F && serial_buff[11] == 0x27) {
+                    } else { /* valid */
                         gps_time_ok = false;
-                        gps_lock_ok = false;
                     }
-                    return UBX_NAV_TIMEUTC;
+
+                    return UBX_NAV_TIMEGPS;
                 } else if ((serial_buff[2] == 0x05) && (serial_buff[3] == 0x00)) {
                     DEBUG_MSG("NOTE: UBX ACK-NAK received\n");
                     return IGNORED;
@@ -462,6 +481,7 @@ enum gps_msg lgw_parse_ubx(const char *serial_buff, size_t buff_size, size_t *ms
     }
 }
 
+
 /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
 enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) {
@@ -487,7 +507,7 @@ enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) {
     } else if (!validate_nmea_checksum(serial_buff, buff_size)) {
         DEBUG_MSG("Warning: invalid NMEA sentence (bad checksum)\n");
         return INVALID;
-    } else if (match_label(serial_buff, "G?RMC", 5, '?')) {
+    } else if (match_label(serial_buff, "$G?RMC", 6, '?')) {
         /*
         NMEA sentence format: $xxRMC,time,status,lat,NS,long,EW,spd,cog,date,mv,mvEW,posMode*cs<CR><LF>
         Valid fix: $GPRMC,083559.34,A,4717.11437,N,00833.91522,E,0.004,77.52,091202,,,A*00
@@ -500,28 +520,35 @@ enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) {
             DEBUG_MSG("Warning: invalid RMC sentence (number of fields)\n");
             return IGNORED;
         }
+
         /* parse GPS status */
-        gps_mod = *(parser_buf + str_index[2]); /* get first character, no need to bother with sscanf */
-        if ((gps_mod != 'N') && (gps_mod != 'A') && (gps_mod != 'D')) {
+        if (str_index[12] < 0) {
             gps_mod = 'N';
+        } else {
+            gps_mod = (char)*(parser_buf + str_index[12]); /* get first character, no need to bother with sscanf */
+            if ((gps_mod != 'N') && (gps_mod != 'A') && (gps_mod != 'D')) {
+                gps_mod = 'N';
+            }
         }
+
         /* parse complete time */
         i = sscanf(parser_buf + str_index[1], "%2hd%2hd%2hd%4f", &gps_hou, &gps_min, &gps_sec, &gps_fra);
         j = sscanf(parser_buf + str_index[9], "%2hd%2hd%2hd", &gps_day, &gps_mon, &gps_yea);
-        if ((i == 3) && (j == 3)) {
+        if ((i == 4) && (j == 3)) {
             if ((gps_mod == 'A') || (gps_mod == 'D')) {
-                gps_lock_ok = true;
+                gps_time_ok = true;
                 DEBUG_MSG("Note: Valid RMC sentence, GPS locked, date: 20%02d-%02d-%02dT%02d:%02d:%06.3fZ\n", gps_yea, gps_mon, gps_day, gps_hou, gps_min, gps_fra + (float)gps_sec);
             } else {
-                gps_lock_ok = false;
+                gps_time_ok = false;
                 DEBUG_MSG("Note: Valid RMC sentence, no satellite fix, estimated date: 20%02d-%02d-%02dT%02d:%02d:%06.3fZ\n", gps_yea, gps_mon, gps_day, gps_hou, gps_min, gps_fra + (float)gps_sec);
             }
         } else {
             /* could not get a valid hour AND date */
+            gps_time_ok = false;
             DEBUG_MSG("Note: Valid RMC sentence, mode %c, no date\n", gps_mod);
         }
         return NMEA_RMC;
-    } else if (match_label(serial_buff, "G?GGA", 5, '?')) {
+    } else if (match_label(serial_buff, "$G?GGA", 6, '?')) {
         /*
         NMEA sentence format: $xxGGA,time,lat,NS,long,EW,quality,numSV,HDOP,alt,M,sep,M,diffAge,diffStation*cs<CR><LF>
         Valid fix: $GPGGA,092725.00,4717.11399,N,00833.91590,E,1,08,1.01,499.6,M,48.0,M,,*5B
@@ -547,8 +574,6 @@ enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) {
         } else {
             /* could not get a valid latitude, longitude AND altitude */
             gps_pos_ok = false;
-            gps_time_ok = false;
-            gps_lock_ok = false;
             DEBUG_MSG("Note: Valid GGA sentence, %d sat, no coordinates\n", gps_sat);
         }
         return NMEA_GGA;
@@ -558,6 +583,7 @@ enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) {
     }
 }
 
+
 /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
 int lgw_gps_get(struct timespec *utc, struct timespec *gps_time, struct coord_s *loc, struct coord_s *err) {
@@ -623,57 +649,74 @@ int lgw_gps_get(struct timespec *utc, struct timespec *gps_time, struct coord_s
 
 /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
-int lgw_gps_sync(struct tref *ref, uint32_t count_us, struct timespec gps_time) {
-    bool update = false;
+int lgw_gps_sync(struct tref *ref, uint32_t count_us, struct timespec utc, struct timespec gps_time) {
     double cnt_diff; /* internal concentrator time difference (in seconds) */
     double utc_diff; /* UTC time difference (in seconds) */
-    double slope = 1.0; /* time slope between new reference and old reference (for sanity check) */
-    static bool calibrating = true;
+    double slope; /* time slope between new reference and old reference (for sanity check) */
+
+    bool aber_n0; /* is the update value for synchronization aberrant or not ? */
+    static bool aber_min1 = false; /* keep track of whether value at sync N-1 was aberrant or not  */
+    static bool aber_min2 = false; /* keep track of whether value at sync N-2 was aberrant or not  */
 
     CHECK_NULL(ref);
 
     /* calculate the slope */
 
-    if (ref->systime != 0) {
-        cnt_diff = (double)(count_us - ref->count_us) / (double)(TS_CPS); /* uncorrected by xtal_err */
-        utc_diff = (double)((gps_time.tv_sec - timezone) - (ref->utc).tv_sec) + (1E-9 * (double)(gps_time.tv_nsec - (ref->utc).tv_nsec));
+    cnt_diff = (double)(count_us - ref->count_us) / (double)(TS_CPS); /* uncorrected by xtal_err */
+    utc_diff = (double)(utc.tv_sec - (ref->utc).tv_sec) + (1E-9 * (double)(utc.tv_nsec - (ref->utc).tv_nsec));
 
-        if (!calibrating && utc_diff < 1.0)
-            return LGW_GPS_ERROR;
-
-        if (cnt_diff != 0 && utc_diff != 0) { // prevent divide by zero
-            slope = cnt_diff/utc_diff;
+    /* detect aberrant points by measuring if slope limits are exceeded */
+    if (utc_diff != 0) { // prevent divide by zero
+        slope = cnt_diff/utc_diff;
+        if ((slope > PLUS_10PPM) || (slope < MINUS_10PPM)) {
+            DEBUG_MSG("Warning: correction range exceeded\n");
+            aber_n0 = true;
         } else {
-            slope = 0.0;
-        }
-
-        update = (slope >= MINUS_10PPM && slope <= PLUS_10PPM);
-
-        if (calibrating && !update && utc_diff > 1.5) {
-            update = true;
-        } else if (update) {
-            calibrating = false;
+            aber_n0 = false;
         }
-
     } else {
-        update = true;
-        slope = 1.0;
+        DEBUG_MSG("Warning: aberrant UTC value for synchronization\n");
+        aber_n0 = true;
     }
 
-    if (update || calibrating) {
+    /* watch if the 3 latest sync point were aberrant or not */
+    if (aber_n0 == false) {
+        /* value no aberrant -> sync with smoothed slope */
+        ref->systime = time(NULL);
+        ref->count_us = count_us;
+        ref->utc.tv_sec = utc.tv_sec;
+        ref->utc.tv_nsec = utc.tv_nsec;
+        ref->gps.tv_sec = gps_time.tv_sec;
+        ref->gps.tv_nsec = gps_time.tv_nsec;
+        ref->xtal_err = slope;
+        aber_min2 = aber_min1;
+        aber_min1 = aber_n0;
+        return LGW_GPS_SUCCESS;
+    } else if (aber_n0 && aber_min1 && aber_min2) {
+        /* 3 successive aberrant values -> sync reset (keep xtal_err) */
         ref->systime = time(NULL);
         ref->count_us = count_us;
-        ref->utc.tv_sec = gps_time.tv_sec - timezone;
-        ref->utc.tv_nsec = gps_time.tv_nsec;
-        ref->gps.tv_sec = gps_time.tv_sec + leap_seconds - 315964800;
+        ref->utc.tv_sec = utc.tv_sec;
+        ref->utc.tv_nsec = utc.tv_nsec;
+        ref->gps.tv_sec = gps_time.tv_sec;
         ref->gps.tv_nsec = gps_time.tv_nsec;
-        ref->xtal_err = (slope >= MINUS_10PPM && slope <= PLUS_10PPM) ? slope : 1.0;
+        /* reset xtal_err only if the present value is out of range */
+        if ((ref->xtal_err > PLUS_10PPM) || (ref->xtal_err < MINUS_10PPM)) {
+            ref->xtal_err = 1.0;
+        }
+        DEBUG_MSG("Warning: 3 successive aberrant sync attempts, sync reset\n");
+        aber_min2 = aber_min1;
+        aber_min1 = aber_n0;
         return LGW_GPS_SUCCESS;
+    } else {
+        /* only 1 or 2 successive aberrant values -> ignore and return an error */
+        aber_min2 = aber_min1;
+        aber_min1 = aber_n0;
+        return LGW_GPS_ERROR;
     }
 
-    return LGW_GPS_ERROR;
+    return LGW_GPS_SUCCESS;
 }
-
 /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
 int lgw_cnt2utc(struct tref ref, uint32_t count_us, struct timespec *utc) {
diff --git a/libloragw/tst/test_loragw_gps.c b/libloragw/tst/test_loragw_gps.c
index 994c0ad..16ea6f0 100644
--- a/libloragw/tst/test_loragw_gps.c
+++ b/libloragw/tst/test_loragw_gps.c
@@ -51,6 +51,8 @@ struct tref ppm_ref;
 /* --- PRIVATE FUNCTIONS DECLARATION ---------------------------------------- */
 
 static void sig_handler(int sigio);
+static void gps_process_sync(void);
+static void gps_process_coords(void);
 
 /* -------------------------------------------------------------------------- */
 /* --- PRIVATE FUNCTIONS DEFINITION ----------------------------------------- */
@@ -63,6 +65,72 @@ 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 -------------------------------------------------------- */
 
@@ -78,9 +146,15 @@ int main()
 
     /* serial variables */
     char serial_buff[128]; /* buffer to receive GPS data */
+    size_t wr_idx = 0;     /* pointer to end of chars in buffer */
+    int gps_tty_dev; /* file descriptor to the serial port of the GNSS module */
 
     /* NMEA/UBX variables */
-    static struct tref time_reference_gps; /* time reference used for GPS <-> timestamp conversion */
+    enum gps_msg latest_msg = UNKNOWN; /* keep track of latest NMEA/UBX message parsed */
+
+    fd_set fds;
+    char delim[4] = "$";
+    char *token[254];
 
     /* configure signal handling */
     sigemptyset(&sigact.sa_mask);
@@ -95,9 +169,9 @@ int main()
     printf("*** Library version information ***\n%s\n***\n", lgw_version_info());
 
     /* Open and configure GPS */
-    i = lgw_gps_enable(&gpsdata, &source);
+    i = lgw_gps_enable("/dev/ttyS0", "ubx7", 0, &gps_tty_dev, 1);
     if (i != LGW_GPS_SUCCESS) {
-        printf("ERROR: IMPOSSIBLE TO ENABLE GPS\n");
+        printf("ERROR: Failed to enable GPS\n");
         exit(EXIT_FAILURE);
     }
 
@@ -124,41 +198,88 @@ int main()
     memset(&ppm_ref, 0, sizeof ppm_ref);
 
     /* loop until user action */
-
     while ((quit_sig != 1) && (exit_sig != 1)) {
-        wait_ms(100);
-        int r = gps_read(&gpsdata, 0, 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:             %lld\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);
+        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);
+            }
         }
     }
 
     /* clean up before leaving */
     if (exit_sig == 1) {
-        lgw_gps_disable(&gpsdata);
+        lgw_gps_disable(gps_tty_dev);
         lgw_stop();
     }