Added GPSD integration to the lora packet forwarder and gateway

1 files changed, 471 insertions, 0 deletions

diff --git a/recipes-connectivity/lora/lora-gateway/lora-gateway-gpsd.patch b/recipes-connectivity/lora/lora-gateway/lora-gateway-gpsd.patch
new file mode 100644
index 0000000..471cd82
--- /dev/null
+++ b/recipes-connectivity/lora/lora-gateway/lora-gateway-gpsd.patch
@@ -0,0 +1,471 @@
+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..d3d1ca1 100644
+--- a/libloragw/src/loragw_gps.c
++++ b/libloragw/src/loragw_gps.c
+@@ -251,109 +251,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;
+-
+-    /* 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
+-    }
+-
+-    /* get actual serial port configuration */
+-    i = tcgetattr(gps_tty_dev, &ttyopt);
+-    if (i != 0) {
+-        DEBUG_MSG("ERROR: IMPOSSIBLE TO GET TTY PORT CONFIGURATION\n");
+-        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");
++int lgw_gps_enable(struct gps_data_t *gpsdata, struct fixsource_t *source) {
++
++    unsigned int flags;
++    fd_set fds;
++    flags = WATCH_ENABLE;
++    flags |= WATCH_RAW;
++    flags |= WATCH_NMEA;
++    gpsd_source_spec(NULL, source);
++
++    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;
+     }
+-    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 +281,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;
+     }
+ 
+@@ -526,7 +432,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,19 +441,19 @@ 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;
+                 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);
+@@ -561,7 +467,7 @@ enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) {
+             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
+diff --git a/libloragw/tst/test_loragw_gps.c b/libloragw/tst/test_loragw_gps.c
+index a4164a3..e4b1546 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();
+     }
+