Apply patches for gpsd and spectral scan utility

4 files changed, 162 insertions, 249 deletions

diff --git a/libloragw/Makefile b/libloragw/Makefile
index 53c33d9..4575bdc 100644
--- a/libloragw/Makefile
+++ b/libloragw/Makefile
@@ -10,14 +10,14 @@ CROSS_COMPILE ?=
 CC := $(CROSS_COMPILE)gcc
 AR := $(CROSS_COMPILE)ar
 
-CFLAGS := -O2 -Wall -Wextra -std=c99 -Iinc -I.
+CFLAGS := -O2 -Wall -Wextra -std=c99 -Iinc -I. -isystem =/usr/include/gps
 
 OBJDIR = obj
 INCLUDES = $(wildcard inc/*.h)
 
 ### linking options
 
-LIBS := -lloragw -lrt -lm
+LIBS := -lloragw -lrt -lm -lgps
 
 ### general build targets
 
diff --git a/libloragw/inc/loragw_gps.h b/libloragw/inc/loragw_gps.h
index 6dbd30b..59b2d37 100644
--- a/libloragw/inc/loragw_gps.h
+++ b/libloragw/inc/loragw_gps.h
@@ -27,9 +27,11 @@ Maintainer: Michael Coracin
 #include <time.h>       /* time library */
 #include <termios.h>    /* speed_t */
 #include <unistd.h>     /* ssize_t */
+#include <gpsd.h>
+#include <gpsdclient.h>
+#include <errno.h>      /* error messages */
 
 #include "config.h"     /* library configuration options (dynamically generated) */
-
 /* -------------------------------------------------------------------------- */
 /* --- PUBLIC TYPES --------------------------------------------------------- */
 
@@ -83,6 +85,7 @@ enum gps_msg {
     UBX_NAV_TIMEUTC  /*!> UTC Time Solution */
 };
 
+// struct gps_data_t gpsdata;
 /* -------------------------------------------------------------------------- */
 /* --- PUBLIC CONSTANTS ----------------------------------------------------- */
 
@@ -99,21 +102,19 @@ enum gps_msg {
 /**
 @brief Configure a GPS module
 
-@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
+@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
 */
-int lgw_gps_enable(char* tty_path, char* gps_familly, speed_t target_brate, int* fd_ptr);
+int lgw_gps_enable(struct gps_data_t *gpsdata, struct fixsource_t *source);
 
 /**
 @brief Restore GPS serial configuration and close serial device
 
-@param fd file descriptor on GPS tty
+@param gpsdata handler for gpsd data
 @return success if the function was able to complete
 */
-int lgw_gps_disable(int fd);
+int lgw_gps_disable(struct gps_data_t *gpsdata);
 
 /**
 @brief Parse messages coming from the GPS system (or other GNSS)
diff --git a/libloragw/src/loragw_gps.c b/libloragw/src/loragw_gps.c
index c0e0ded..3aad031 100644
--- a/libloragw/src/loragw_gps.c
+++ b/libloragw/src/loragw_gps.c
@@ -84,6 +84,7 @@ static double gps_mlo = 0.0; /* minutes of longitude */
 static char gps_olo = 0; /* orientation (E-W) of longitude */
 static short gps_alt = 0; /* altitude */
 static bool gps_pos_ok = false;
+static bool gps_lock_ok = false;
 
 static char gps_mod = 'N'; /* GPS mode (N no fix, A autonomous, D differential) */
 static short gps_sat = 0; /* number of satellites used for fix */
@@ -251,109 +252,22 @@ int str_chop(char *s, int buff_size, char separator, int *idx_ary, int max_idx)
 /* -------------------------------------------------------------------------- */
 /* --- PUBLIC FUNCTIONS DEFINITION ------------------------------------------ */
 
-int lgw_gps_enable(char *tty_path, char *gps_family, speed_t target_brate, int *fd_ptr) {
-    int i;
-    struct termios ttyopt; /* serial port options */
-    int gps_tty_dev; /* file descriptor to the serial port of the GNSS module */
-    uint8_t ubx_cmd_timegps[UBX_MSG_NAVTIMEGPS_LEN] = {
-                    0xB5, 0x62, /* UBX Sync Chars */
-                    0x06, 0x01, /* CFG-MSG Class/ID */
-                    0x08, 0x00, /* Payload length */
-                    0x01, 0x20, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, /* Enable NAV-TIMEGPS output on serial */
-                    0x32, 0x94 }; /* Checksum */
-    ssize_t num_written;
-
-    /* check input parameters */
-    CHECK_NULL(tty_path);
-    CHECK_NULL(fd_ptr);
-
-    /* open TTY device */
-    gps_tty_dev = open(tty_path, O_RDWR | O_NOCTTY);
-    if (gps_tty_dev <= 0) {
-        DEBUG_MSG("ERROR: TTY PORT FAIL TO OPEN, CHECK PATH AND ACCESS RIGHTS\n");
-        return LGW_GPS_ERROR;
-    }
-    *fd_ptr = gps_tty_dev;
+int lgw_gps_enable(struct gps_data_t *gpsdata, struct fixsource_t *source) {
 
-    /* manage the different GPS modules families */
-    if (gps_family == NULL) {
-        DEBUG_MSG("WARNING: this version of GPS module may not be supported\n");
-    } else if (strncmp(gps_family, "ubx7", 4) != 0) {
-        /* The current implementation relies on proprietary messages from U-Blox */
-        /* GPS modules (UBX, NAV-TIMEGPS...) and has only be tested with a u-blox 7. */
-        /* Those messages allow to get NATIVE GPS time (no leap seconds) required */
-        /* for class-B handling and GPS synchronization */
-        /* see lgw_parse_ubx() function for details */
-        DEBUG_MSG("WARNING: this version of GPS module may not be supported\n");
-    }
-
-    /* manage the target bitrate */
-    if (target_brate != 0) {
-        DEBUG_MSG("WARNING: target_brate parameter ignored for now\n"); // TODO
-    }
+    unsigned int flags;
+    fd_set fds;
+    flags = WATCH_ENABLE;
+    flags |= WATCH_RAW;
+    flags |= WATCH_NMEA;
+    gpsd_source_spec(NULL, source);
 
-    /* get actual serial port configuration */
-    i = tcgetattr(gps_tty_dev, &ttyopt);
-    if (i != 0) {
-        DEBUG_MSG("ERROR: IMPOSSIBLE TO GET TTY PORT CONFIGURATION\n");
+    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);
         return LGW_GPS_ERROR;
     }
 
-    /* Save current serial port configuration for restoring later */
-    memcpy(&ttyopt_restore, &ttyopt, sizeof ttyopt);
-
-    /* update baudrates */
-    cfsetispeed(&ttyopt, DEFAULT_BAUDRATE);
-    cfsetospeed(&ttyopt, DEFAULT_BAUDRATE);
-
-    /* update terminal parameters */
-    /* The following configuration should allow to:
-            - Get ASCII NMEA messages
-            - Get UBX binary messages
-            - Send UBX binary commands
-        Note: as binary data have to be read/written, we need to disable
-              various character processing to avoid loosing data */
-    /* Control Modes */
-    ttyopt.c_cflag |= CLOCAL;  /* local connection, no modem control */
-    ttyopt.c_cflag |= CREAD;   /* enable receiving characters */
-    ttyopt.c_cflag |= CS8;     /* 8 bit frames */
-    ttyopt.c_cflag &= ~PARENB; /* no parity */
-    ttyopt.c_cflag &= ~CSTOPB; /* one stop bit */
-    /* Input Modes */
-    ttyopt.c_iflag |= IGNPAR;  /* ignore bytes with parity errors */
-    ttyopt.c_iflag &= ~ICRNL;  /* do not map CR to NL on input*/
-    ttyopt.c_iflag &= ~IGNCR;  /* do not ignore carriage return on input */
-    ttyopt.c_iflag &= ~IXON;   /* disable Start/Stop output control */
-    ttyopt.c_iflag &= ~IXOFF;  /* do not send Start/Stop characters */
-    /* Output Modes */
-    ttyopt.c_oflag = 0;        /* disable everything on output as we only write binary */
-    /* Local Modes */
-    ttyopt.c_lflag &= ~ICANON; /* disable canonical input - cannot use with binary input */
-    ttyopt.c_lflag &= ~ISIG;   /* disable check for INTR, QUIT, SUSP special characters */
-    ttyopt.c_lflag &= ~IEXTEN; /* disable any special control character */
-    ttyopt.c_lflag &= ~ECHO;   /* do not echo back every character typed */
-    ttyopt.c_lflag &= ~ECHOE;  /* does not erase the last character in current line */
-    ttyopt.c_lflag &= ~ECHOK;  /* do not echo NL after KILL character */
-
-    /* settings for non-canonical mode
-       read will block for until the lesser of VMIN or requested chars have been received */
-    ttyopt.c_cc[VMIN]  = LGW_GPS_MIN_MSG_SIZE;
-    ttyopt.c_cc[VTIME] = 0;
-
-    /* set new serial ports parameters */
-    i = tcsetattr(gps_tty_dev, TCSANOW, &ttyopt);
-    if (i != 0){
-        DEBUG_MSG("ERROR: IMPOSSIBLE TO UPDATE TTY PORT CONFIGURATION\n");
-        return LGW_GPS_ERROR;
-    }
-    tcflush(gps_tty_dev, TCIOFLUSH);
-
-    /* Send UBX CFG NAV-TIMEGPS message to tell GPS module to output native GPS time */
-    /* This is a binary message, serial port has to be properly configured to handle this */
-    num_written = write (gps_tty_dev, ubx_cmd_timegps, UBX_MSG_NAVTIMEGPS_LEN);
-    if (num_written != UBX_MSG_NAVTIMEGPS_LEN) {
-        DEBUG_MSG("ERROR: Failed to write on serial port (written=%d)\n", (int) num_written);
-    }
+    (void)gps_stream(gpsdata, flags, source->device);
 
     /* get timezone info */
     tzset();
@@ -368,20 +282,13 @@ int lgw_gps_enable(char *tty_path, char *gps_family, speed_t target_brate, int *
 
 /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
-int lgw_gps_disable(int fd) {
+int lgw_gps_disable(struct gps_data_t *gpsdata) {
     int i;
 
-    /* restore serial ports parameters */
-    i = tcsetattr(fd, TCSANOW, &ttyopt_restore);
-    if (i != 0){
-        DEBUG_MSG("ERROR: IMPOSSIBLE TO RESTORE TTY PORT CONFIGURATION\n");
-        return LGW_GPS_ERROR;
-    }
-    tcflush(fd, TCIOFLUSH);
-
-    i = close(fd);
-    if (i <= 0) {
-        DEBUG_MSG("ERROR: TTY PORT FAIL TO CLOSE\n");
+    /* ends the session */
+    i = gps_close(gpsdata);
+    if (i != 0) {
+        DEBUG_MSG("ERROR: GPSD FAILED TO CLOSE\n");
         return LGW_GPS_ERROR;
     }
 
@@ -457,7 +364,8 @@ 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 */
                         {
@@ -472,11 +380,24 @@ 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
-                    } else { /* valid */
+                        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) {
+			gps_time_ok = false;
+			gps_lock_ok = false;
                     }
 
-                    return UBX_NAV_TIMEGPS;
+                    return UBX_NAV_TIMEUTC;
                 } else if ((serial_buff[2] == 0x05) && (serial_buff[3] == 0x00)) {
                     DEBUG_MSG("NOTE: UBX ACK-NAK received\n");
                     return IGNORED;
@@ -526,7 +447,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", 6, '?')) {
+    } else if (match_label(serial_buff, "G?RMC", 5, '?')) {
         /*
         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
@@ -535,33 +456,32 @@ enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) {
         memcpy(parser_buf, serial_buff, buff_size);
         parser_buf[buff_size] = '\0';
         nb_fields = str_chop(parser_buf, buff_size, ',', str_index, ARRAY_SIZE(str_index));
-        if (nb_fields != 13) {
+        if (nb_fields != 12) {
             DEBUG_MSG("Warning: invalid RMC sentence (number of fields)\n");
             return IGNORED;
         }
         /* parse GPS status */
-        gps_mod = *(parser_buf + str_index[12]); /* get first character, no need to bother with sscanf */
+        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')) {
             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 == 4) && (j == 3)) {
+        if ((i == 3) && (j == 3)) {
             if ((gps_mod == 'A') || (gps_mod == 'D')) {
-                gps_time_ok = true;
+                gps_lock_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_time_ok = false;
+                gps_lock_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", 6, '?')) {
+    } else if (match_label(serial_buff, "G?GGA", 5, '?')) {
         /*
         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
@@ -587,6 +507,8 @@ 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;
@@ -662,38 +584,49 @@ 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 utc, struct timespec gps_time) {
+    bool update = false;
     double cnt_diff; /* internal concentrator time difference (in seconds) */
     double utc_diff; /* UTC time difference (in seconds) */
-    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  */
+    double slope = 1.0; /* time slope between new reference and old reference (for sanity check) */
+    static bool calibrating = true;
 
     CHECK_NULL(ref);
 
     /* calculate the slope */
 
-    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 (ref->systime != 0) {
+
+        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));
 
-    /* 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;
+        if (cnt_diff != 0 && utc_diff != 0) { // prevent divide by zero
+            slope = cnt_diff/utc_diff;
         } else {
-            aber_n0 = false;
+           slope = 0.0;
+        }
+
+        if (gps_lock_ok && gps_time_ok && cnt_diff > 1.5) {
+           update = true;
         }
+
+        update = (slope >= MINUS_10PPM && slope <= PLUS_10PPM);
+
+        if (!calibrating && utc_diff < 10.0) {
+            return LGW_GPS_ERROR;
+        }
+
+        if (calibrating && !update && utc_diff > 1.5) {
+           update = true;
+        } else if (update) {
+            calibrating = false;
+        }
+
     } else {
-        DEBUG_MSG("Warning: aberrant UTC value for synchronization\n");
-        aber_n0 = true;
+       update = true;
+        slope = 0.0;
     }
 
-    /* watch if the 3 latest sync point were aberrant or not */
-    if (aber_n0 == false) {
-        /* value no aberrant -> sync with smoothed slope */
+    if (update || calibrating) {
         ref->systime = time(NULL);
         ref->count_us = count_us;
         ref->utc.tv_sec = utc.tv_sec;
@@ -701,33 +634,10 @@ int lgw_gps_sync(struct tref *ref, uint32_t count_us, struct timespec utc, struc
         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 = 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;
-        /* 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_SUCCESS;
+    return LGW_GPS_ERROR;
 }
 
 /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
diff --git a/libloragw/tst/test_loragw_gps.c b/libloragw/tst/test_loragw_gps.c
index a4164a3..6e644f8 100644
--- a/libloragw/tst/test_loragw_gps.c
+++ b/libloragw/tst/test_loragw_gps.c
@@ -31,6 +31,8 @@ Maintainer: Michael Coracin
 #include <signal.h>     /* sigaction */
 #include <stdlib.h>     /* exit */
 #include <unistd.h>     /* read */
+#include <gps.h>
+#include <gpsd.h>
 
 #include "loragw_hal.h"
 #include "loragw_gps.h"
@@ -41,7 +43,8 @@ Maintainer: Michael Coracin
 
 static int exit_sig = 0; /* 1 -> application terminates cleanly (shut down hardware, close open files, etc) */
 static int quit_sig = 0; /* 1 -> application terminates without shutting down the hardware */
-
+static struct gps_data_t gpsdata;
+static struct fixsource_t source;
 struct tref ppm_ref;
 
 /* -------------------------------------------------------------------------- */
@@ -149,6 +152,10 @@ int main()
     /* NMEA/UBX variables */
     enum gps_msg latest_msg; /* keep track of latest NMEA/UBX message parsed */
 
+    fd_set fds;
+    char delim[4] = "$";
+    char *token[254];
+
     /* configure signal handling */
     sigemptyset(&sigact.sa_mask);
     sigact.sa_flags = 0;
@@ -162,7 +169,7 @@ int main()
     printf("*** Library version information ***\n%s\n***\n", lgw_version_info());
 
     /* Open and configure GPS */
-    i = lgw_gps_enable("/dev/ttyAMA0", "ubx7", 0, &gps_tty_dev);
+    i = lgw_gps_enable(&gpsdata, &source);
     if (i != LGW_GPS_SUCCESS) {
         printf("ERROR: IMPOSSIBLE TO ENABLE GPS\n");
         exit(EXIT_FAILURE);
@@ -192,92 +199,87 @@ int main()
 
     /* loop until user action */
     while ((quit_sig != 1) && (exit_sig != 1)) {
-        size_t rd_idx = 0;
-        size_t frame_end_idx = 0;
-
-        /* blocking non-canonical read on serial port */
-        ssize_t nb_char = read(gps_tty_dev, serial_buff + wr_idx, LGW_GPS_MIN_MSG_SIZE);
-        if (nb_char <= 0) {
-            printf("WARNING: [gps] read() returned value %d\n", nb_char);
+        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;
-        }
-        wr_idx += (size_t)nb_char;
 
-        /*******************************************
-         * Scan buffer for UBX/NMEA sync chars and *
-         * attempt to decode frame if one is found *
-         *******************************************/
-        while (rd_idx < wr_idx) {
+        /* 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;
-
-            /* Scan buffer for UBX sync char */
-            if (serial_buff[rd_idx] == LGW_GPS_UBX_SYNC_CHAR) {
-
-                /***********************
-                 * Found UBX sync char *
-                 ***********************/
-                latest_msg = lgw_parse_ubx(&serial_buff[rd_idx], (wr_idx - rd_idx), &frame_size);
-
-                if (frame_size > 0) {
-                    if (latest_msg == INCOMPLETE) {
-                        /* UBX header found but frame appears to be missing bytes */
-                        frame_size = 0;
-                    } else if (latest_msg == INVALID) {
-                        /* message header received but message appears to be corrupted */
-                        printf("WARNING: [gps] could not get a valid message from GPS (no time)\n");
-                        frame_size = 0;
-                    } else if (latest_msg == UBX_NAV_TIMEGPS) {
-                        printf("\n~~ UBX NAV-TIMEGPS sentence, triggering synchronization attempt ~~\n");
-                        gps_process_sync();
+            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);
                     }
-                }
-            } else if(serial_buff[rd_idx] == LGW_GPS_NMEA_SYNC_CHAR) {
-                /************************
-                 * Found NMEA sync char *
-                 ************************/
-                /* scan for NMEA end marker (LF = 0x0a) */
-                char* nmea_end_ptr = memchr(&serial_buff[rd_idx],(int)0x0a, (wr_idx - rd_idx));
-
-                if (nmea_end_ptr) {
-                    /* found end marker */
-                    frame_size = nmea_end_ptr - &serial_buff[rd_idx] + 1;
-                    latest_msg = lgw_parse_nmea(&serial_buff[rd_idx], frame_size);
-
-                    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();
+                    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();
+                            }
+                        }
                     }
                 }
             }
-
-            if (frame_size > 0) {
-                /* At this point message is a checksum verified frame
-                   we're processed or ignored. Remove frame from buffer */
-                rd_idx += frame_size;
-                frame_end_idx = rd_idx;
+        } else {
+            if (r == -1) {
+                if (errno == EAGAIN)
+                    continue;
+                else {
+                    printf(stderr, "gpspipe: read error %s(%d)\n", strerror(errno), errno);
+                    exit(EXIT_FAILURE);
+                }
             } else {
-                rd_idx++;
+                exit(EXIT_SUCCESS);
             }
-        } /* ...for(rd_idx = 0... */
-
-        if (frame_end_idx) {
-          /* Frames have been processed. Remove bytes to end of last processed frame */
-          memcpy(serial_buff,&serial_buff[frame_end_idx],wr_idx - frame_end_idx);
-          wr_idx -= frame_end_idx;
-        } /* ...for(rd_idx = 0... */
-
-        /* Prevent buffer overflow */
-        if ((sizeof(serial_buff) - wr_idx) < LGW_GPS_MIN_MSG_SIZE) {
-            memcpy(serial_buff,&serial_buff[LGW_GPS_MIN_MSG_SIZE],wr_idx - LGW_GPS_MIN_MSG_SIZE);
-            wr_idx -= LGW_GPS_MIN_MSG_SIZE;
         }
     }
 
     /* clean up before leaving */
     if (exit_sig == 1) {
-        lgw_gps_disable(gps_tty_dev);
+        lgw_gps_disable(&gpsdata);
         lgw_stop();
     }