Updated gps handlers to use gpsd stream

3 files changed, 168 insertions, 150 deletions

diff --git a/libloragw/inc/loragw_gps.h b/libloragw/inc/loragw_gps.h
index 8041e43..9e3aa27 100644
--- a/libloragw/inc/loragw_gps.h
+++ b/libloragw/inc/loragw_gps.h
@@ -22,7 +22,6 @@ Maintainer: Michael Coracin
 /* -------------------------------------------------------------------------- */
 /* --- DEPENDANCIES --------------------------------------------------------- */
 
-#define _GNU_SOURCE
 #include <stdint.h>     /* C99 types */
 #include <time.h>       /* time library */
 #include <termios.h>    /* speed_t */
@@ -31,6 +30,7 @@ Maintainer: Michael Coracin
 #include <gpsdclient.h>
 #include <errno.h>      /* error messages */
 #include <sys/stat.h>
+#include <sys/socket.h>
 
 #include "config.h"     /* library configuration options (dynamically generated) */
 /* -------------------------------------------------------------------------- */
@@ -90,11 +90,10 @@ enum gps_state {
     GPS_UNKNOWN,
     GPS_RUNNING,
     GPS_LOST,
-    GPS_RETRYING
+    GPS_RECONNECTING
 };
 
 
-// struct gps_data_t gpsdata;
 /* -------------------------------------------------------------------------- */
 /* --- PUBLIC CONSTANTS ----------------------------------------------------- */
 
@@ -109,25 +108,29 @@ enum gps_state {
 /* --- PUBLIC FUNCTIONS PROTOTYPES ------------------------------------------ */
 
 /**
-@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
+@brief Enable GPSD
 @return success if the function was able to connect and configure a GPS module
 */
-int lgw_gps_enable(char* tty_path, char* gps_familly, speed_t target_brate, int* fd_ptr, int slot);
+int lgw_gps_enable();
 
 /**
-@brief Restore GPS serial configuration and close serial device
+@brief Disable GPSD
+@return success if the function was able to disable the gpsd connection
+*/
+int lgw_gps_disable();
 
-@param fd file descriptor on GPS tty
-@return success if the function was able to complete
+/**
+@brief Prepare for a blocking read
+@return 1 if we have data waiting, 0 if it timed out
 */
-int lgw_gps_disable(int fd);
+int lgw_gps_data_ready();
 
 /**
+@brief Get data from gpsd stream
+@return size of message received
+*/
+int lgw_gps_stream(char *message, size_t len);
+/**
 @brief Get updated leap seconds
 
 @return success if the function was able to get the new leap seconds
diff --git a/libloragw/src/loragw_gps.c b/libloragw/src/loragw_gps.c
index 43ec0bc..e13b85b 100644
--- a/libloragw/src/loragw_gps.c
+++ b/libloragw/src/loragw_gps.c
@@ -19,7 +19,6 @@ Maintainer: Michael Coracin
 /* -------------------------------------------------------------------------- */
 /* --- DEPENDANCIES --------------------------------------------------------- */
 
-#define _GNU_SOURCE     /* needed for qsort_r to be defined */
 #include <stdint.h>     /* C99 types */
 #include <stdbool.h>    /* bool type */
 #include <stdio.h>      /* printf fprintf */
@@ -59,19 +58,7 @@ 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 "\"";
+static struct gps_data_t gpsdata;
 
 /* -------------------------------------------------------------------------- */
 /* --- PRIVATE VARIABLES ---------------------------------------------------- */
@@ -267,40 +254,18 @@ 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 slot) {
-    int i;
-    int gps_tty_dev; /* file descriptor to the serial port of the GNSS module */
-
-    /* check input parameters */
-    CHECK_NULL(tty_path);
-    CHECK_NULL(fd_ptr);
-
-    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 {
-        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 );
+int lgw_gps_enable() {
+    unsigned int flags;
+    struct fixsource_t source;
+    flags |= WATCH_RAW; /* super-raw data (gps binary) */
+    flags |= WATCH_NMEA; /* raw NMEA */
+    gpsd_source_spec(NULL, &source);
 
-    if (gps_tty_dev <= 0) {
-        printf("ERROR: TTY PORT FAIL TO OPEN, CHECK PATH AND ACCESS RIGHTS\n");
+    if (gps_open(source.server, source.port, &gpsdata) != 0) {
         return LGW_GPS_ERROR;
     }
-    *fd_ptr = gps_tty_dev;
+
+    (void)gps_stream(&gpsdata, flags, source.device);
 
     /* get timezone info */
     tzset();
@@ -315,23 +280,20 @@ int lgw_gps_enable(char *tty_path, char *gps_family, speed_t target_brate, int *
 
 /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
-int lgw_gps_disable(int fd) {
-    int i;
+int lgw_gps_disable() {
+    (void)gps_close(&gpsdata);
+}
 
-    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");
+int lgw_gps_data_ready() {
+    return nanowait(gpsdata.gps_fd, NS_IN_SEC);
+}
 
-    if (i != 0) {
-        DEBUG_MSG("ERROR: TTY PORT FAIL TO CLOSE - %s\n", strerror(errno));
-        return LGW_GPS_ERROR;
-    }
+/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
-    return LGW_GPS_SUCCESS;
+int lgw_gps_stream(char *message, size_t len) {
+    return recv(gpsdata.gps_fd, message, len, 0);
 }
 
 /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
diff --git a/libloragw/tst/test_loragw_gps.c b/libloragw/tst/test_loragw_gps.c
index 16ea6f0..05ec53c 100644
--- a/libloragw/tst/test_loragw_gps.c
+++ b/libloragw/tst/test_loragw_gps.c
@@ -31,8 +31,6 @@ 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"
@@ -43,8 +41,7 @@ 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;
 
 /* -------------------------------------------------------------------------- */
@@ -150,11 +147,7 @@ int main()
     int gps_tty_dev; /* file descriptor to the serial port of the GNSS module */
 
     /* NMEA/UBX variables */
-    enum gps_msg latest_msg = UNKNOWN; /* keep track of latest NMEA/UBX message parsed */
-
-    fd_set fds;
-    char delim[4] = "$";
-    char *token[254];
+    enum gps_msg latest_msg; /* keep track of latest NMEA/UBX message parsed */
 
     /* configure signal handling */
     sigemptyset(&sigact.sa_mask);
@@ -169,7 +162,7 @@ int main()
     printf("*** Library version information ***\n%s\n***\n", lgw_version_info());
 
     /* Open and configure GPS */
-    i = lgw_gps_enable("/dev/ttyS0", "ubx7", 0, &gps_tty_dev, 1);
+    i = lgw_gps_enable();
     if (i != LGW_GPS_SUCCESS) {
         printf("ERROR: Failed to enable GPS\n");
         exit(EXIT_FAILURE);
@@ -197,89 +190,149 @@ int main()
     memset(serial_buff, 0, sizeof serial_buff);
     memset(&ppm_ref, 0, sizeof ppm_ref);
 
+    uint8_t read_fail_count = 0;
+    uint8_t empty_packet_count = 0;
+    enum gps_state state = GPS_RUNNING;
+
     /* 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);
+        size_t rd_idx = 0;
+        size_t frame_end_idx = 0;
+        ssize_t nb_char = 0;
+
+        switch (state) {
+            case GPS_LOST: {
+                i = lgw_gps_disable();
+                if (i == LGW_HAL_SUCCESS) {
+                    printf("INFO: GPS closed successfully\n");
+                } else {
+                    printf("WARNING: failed to close GPS successfully\n");
+                }
+                empty_packet_count = 0;
+                read_fail_count = 0;
+                state = GPS_RECONNECTING;
+                wait_ms(5000);
+                continue;
+            }
+            case GPS_RECONNECTING: {
+                /* try and reestablish connection */
+                i = lgw_gps_enable();
+                if (i != LGW_GPS_SUCCESS) {
+                    printf("WARNING: [main] impossible to open for GPS sync (Check GPSD)\n");
+                    wait_ms(2000);
+                    continue;
+                } else {
+                    printf("INFO: [main] GPSD polling open for GPS synchronization\n");
+                    state = GPS_RUNNING;
+                    wait_ms(1000);
+                    continue;
+                }
+            }
+            case GPS_RUNNING: {
+                int result = lgw_gps_data_ready();
+
+                if (result != 1) {
+                    empty_packet_count++;
+                    if (empty_packet_count > 9) {
+                        state = GPS_LOST;
                     }
-                    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();
-                        }
+                    continue;
+                }
+                /* reading directly from the socket avoids decode overhead */
+                nb_char = lgw_gps_stream(serial_buff + wr_idx, sizeof(serial_buff) - wr_idx);
+                if (nb_char <= 0) {
+                    printf("WARNING: [gps] read() returned value %zd\n", nb_char);
+                    read_fail_count++;
+                    if (read_fail_count > 9) {
+                        state = GPS_LOST;
+                        continue;
                     }
-                } 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);
+                }
+                break;
+            }
+            default:
+                printf("ERROR: Unknown GPS state, resetting\n");
+                state = GPS_LOST;
+                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) {
+            size_t frame_size = 0;
+
+            /* Scan buffer for UBX sync char */
+            if (serial_buff[rd_idx] == (char)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 (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(serial_buff[rd_idx] == (char)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();
                     }
                 }
             }
-        } else {
-            if (r == -1) {
-                if (errno == EAGAIN)
-                    continue;
-                else {
-                    printf(stderr, "gpspipe: read error %s(%d)\n", strerror(errno), errno);
-                    exit(EXIT_FAILURE);
-                }
+
+            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 {
-                exit(EXIT_SUCCESS);
+                rd_idx++;
             }
+        } /* ...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();
         lgw_stop();
     }