From c9df9ed22788a5ebca6fef270d9377e74737be1b Mon Sep 17 00:00:00 2001 From: Jason Reiss Date: Thu, 19 Sep 2019 08:25:29 -0500 Subject: Apply patches for gpsd and spectral scan utility --- libloragw/tst/test_loragw_gps.c | 150 ++++++++++++++++++++-------------------- 1 file changed, 76 insertions(+), 74 deletions(-) (limited to 'libloragw/tst') 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 /* 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(); } -- cgit v1.2.3