Apply patches for gpsd and spectral scan utility

1 files changed, 76 insertions, 74 deletions

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();
     }