From 3b7195f39bb90e269e008adf2736441943ee7f63 Mon Sep 17 00:00:00 2001 From: Harsh Sharma Date: Thu, 5 May 2022 17:07:33 -0500 Subject: Updated gps handlers to use gpsd stream --- libloragw/inc/loragw_gps.h | 31 +++--- libloragw/src/loragw_gps.c | 78 ++++----------- libloragw/tst/test_loragw_gps.c | 209 +++++++++++++++++++++++++--------------- 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 /* C99 types */ #include /* time library */ #include /* speed_t */ @@ -31,6 +30,7 @@ Maintainer: Michael Coracin #include #include /* error messages */ #include +#include #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,24 +108,28 @@ 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 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 /* C99 types */ #include /* bool type */ #include /* 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 /* sigaction */ #include /* exit */ #include /* read */ -#include -#include #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(); } -- cgit v1.2.3