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 library */ #include /* speed_t */ #include /* ssize_t */ +#include +#include +#include /* 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 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 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 /* sigaction */ #include /* exit */ #include /* read */ +#include +#include #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(); }