From 54b764826671d750d40d1b717f2a171a093aaafb Mon Sep 17 00:00:00 2001 From: Harsh Sharma Date: Thu, 21 Apr 2022 16:22:04 -0500 Subject: Changed gps functions to use gpspipe --- libloragw/inc/loragw_gps.h | 17 ++-- libloragw/src/loragw_gps.c | 191 ++++++++++++++++++++++++---------------- libloragw/tst/test_loragw_gps.c | 185 +++++++++++++++++++++++++++++++------- 3 files changed, 280 insertions(+), 113 deletions(-) diff --git a/libloragw/inc/loragw_gps.h b/libloragw/inc/loragw_gps.h index 0714974..3accae2 100644 --- a/libloragw/inc/loragw_gps.h +++ b/libloragw/inc/loragw_gps.h @@ -30,6 +30,7 @@ Maintainer: Michael Coracin #include #include #include /* error messages */ +#include #include "config.h" /* library configuration options (dynamically generated) */ /* -------------------------------------------------------------------------- */ @@ -102,19 +103,21 @@ enum gps_msg { /** @brief 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 +@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 */ -int lgw_gps_enable(struct gps_data_t *gpsdata, struct fixsource_t *source); +int lgw_gps_enable(char* tty_path, char* gps_familly, speed_t target_brate, int* fd_ptr, int slot); /** @brief Restore GPS serial configuration and close serial device -@param gpsdata handler for gpsd data +@param fd file descriptor on GPS tty @return success if the function was able to complete */ -int lgw_gps_disable(struct gps_data_t *gpsdata); +int lgw_gps_disable(int fd); /** @brief Get updated leap seconds @@ -180,7 +183,7 @@ int lgw_gps_get(struct timespec *utc, struct timespec *gps_time, struct coord_s Set systime to 0 in ref to trigger initial synchronization. */ -int lgw_gps_sync(struct tref *ref, uint32_t count_us, struct timespec gps_time); +int lgw_gps_sync(struct tref *ref, uint32_t count_us, struct timespec utc, struct timespec gps_time); /** @brief Convert concentrator timestamp counter value to UTC time diff --git a/libloragw/src/loragw_gps.c b/libloragw/src/loragw_gps.c index 2698c4b..43ec0bc 100644 --- a/libloragw/src/loragw_gps.c +++ b/libloragw/src/loragw_gps.c @@ -59,6 +59,20 @@ 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 "\""; + /* -------------------------------------------------------------------------- */ /* --- PRIVATE VARIABLES ---------------------------------------------------- */ @@ -253,26 +267,40 @@ int str_chop(char *s, int buff_size, char separator, int *idx_ary, int max_idx) /* -------------------------------------------------------------------------- */ /* --- PUBLIC FUNCTIONS DEFINITION ------------------------------------------ */ -int lgw_gps_enable(struct gps_data_t *gpsdata, struct fixsource_t *source) { +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 */ - unsigned int flags = WATCH_ENABLE| WATCH_JSON; - gpsd_source_spec(NULL, source); + /* check input parameters */ + CHECK_NULL(tty_path); + CHECK_NULL(fd_ptr); - /* Update leap seconds from gpspipe */ - if (lgw_get_leap_seconds() != LGW_GPS_SUCCESS) { - DEBUG_MSG("WARNING: Could not update leap seconds, using default value: %d\n", leap_seconds); + 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 { - DEBUG_MSG("Updated leap seconds to: %d\n", leap_seconds); + 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 ); - 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); + if (gps_tty_dev <= 0) { + printf("ERROR: TTY PORT FAIL TO OPEN, CHECK PATH AND ACCESS RIGHTS\n"); return LGW_GPS_ERROR; } - - (void)gps_stream(gpsdata, flags, source->device); + *fd_ptr = gps_tty_dev; /* get timezone info */ tzset(); @@ -287,15 +315,19 @@ int lgw_gps_enable(struct gps_data_t *gpsdata, struct fixsource_t *source) { /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ -int lgw_gps_disable(struct gps_data_t *gpsdata) { +int lgw_gps_disable(int fd) { int i; - (void) gps_stream(&gps_data, WATCH_DISABLE, NULL); + 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"); - /* ends the session */ - i = gps_close(gpsdata); if (i != 0) { - DEBUG_MSG("ERROR: GPSD FAILED TO CLOSE\n"); + DEBUG_MSG("ERROR: TTY PORT FAIL TO CLOSE - %s\n", strerror(errno)); return LGW_GPS_ERROR; } @@ -405,8 +437,7 @@ enum gps_msg lgw_parse_ubx(const char *serial_buff, size_t buff_size, size_t *ms gps_week = (uint8_t)serial_buff[14]; gps_week |= (uint8_t)serial_buff[15] << 8; /* GPS week number */ - - + gps_time_ok = true; #if 0 /* For debug */ { @@ -421,23 +452,11 @@ enum gps_msg lgw_parse_ubx(const char *serial_buff, size_t buff_size, size_t *ms printf(" GPS time = %02d:%02d:%02d\n", ubx_gps_hou, ubx_gps_min, ubx_gps_sec); } #endif - if (gps_lock_ok) - gps_time_ok = true; - - return UBX_NAV_TIMEGPS; - } else { - gps_time_ok = false; - return INVALID; - } - } else if ((serial_buff[2] == 0x01) && (serial_buff[3] == 0x04)) { - if (serial_buff[10] == 0x0F && serial_buff[11] == 0x27 - && serial_buff[10] == 0x0F && serial_buff[11] == 0x27 - && serial_buff[10] == 0x0F && serial_buff[11] == 0x27 - && serial_buff[10] == 0x0F && serial_buff[11] == 0x27) { + } else { /* valid */ gps_time_ok = false; - gps_lock_ok = false; } - return UBX_NAV_TIMEUTC; + + return UBX_NAV_TIMEGPS; } else if ((serial_buff[2] == 0x05) && (serial_buff[3] == 0x00)) { DEBUG_MSG("NOTE: UBX ACK-NAK received\n"); return IGNORED; @@ -462,6 +481,7 @@ enum gps_msg lgw_parse_ubx(const char *serial_buff, size_t buff_size, size_t *ms } } + /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) { @@ -487,7 +507,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", 5, '?')) { + } else if (match_label(serial_buff, "$G?RMC", 6, '?')) { /* 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 @@ -500,28 +520,35 @@ enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) { DEBUG_MSG("Warning: invalid RMC sentence (number of fields)\n"); return IGNORED; } + /* parse GPS status */ - 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')) { + if (str_index[12] < 0) { gps_mod = 'N'; + } else { + gps_mod = (char)*(parser_buf + str_index[12]); /* 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 == 3) && (j == 3)) { + if ((i == 4) && (j == 3)) { if ((gps_mod == 'A') || (gps_mod == 'D')) { - gps_lock_ok = true; + 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); } else { - gps_lock_ok = false; + gps_time_ok = false; DEBUG_MSG("Note: Valid RMC sentence, no satellite fix, estimated 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); } } else { /* could not get a valid hour AND date */ + gps_time_ok = false; DEBUG_MSG("Note: Valid RMC sentence, mode %c, no date\n", gps_mod); } return NMEA_RMC; - } else if (match_label(serial_buff, "G?GGA", 5, '?')) { + } else if (match_label(serial_buff, "$G?GGA", 6, '?')) { /* 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 @@ -547,8 +574,6 @@ enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) { } else { /* could not get a valid latitude, longitude AND altitude */ gps_pos_ok = false; - gps_time_ok = false; - gps_lock_ok = false; DEBUG_MSG("Note: Valid GGA sentence, %d sat, no coordinates\n", gps_sat); } return NMEA_GGA; @@ -558,6 +583,7 @@ enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) { } } + /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ int lgw_gps_get(struct timespec *utc, struct timespec *gps_time, struct coord_s *loc, struct coord_s *err) { @@ -623,57 +649,74 @@ int lgw_gps_get(struct timespec *utc, struct timespec *gps_time, struct coord_s /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ -int lgw_gps_sync(struct tref *ref, uint32_t count_us, struct timespec gps_time) { - bool update = false; +int lgw_gps_sync(struct tref *ref, uint32_t count_us, struct timespec utc, struct timespec gps_time) { double cnt_diff; /* internal concentrator time difference (in seconds) */ double utc_diff; /* UTC time difference (in seconds) */ - double slope = 1.0; /* time slope between new reference and old reference (for sanity check) */ - static bool calibrating = true; + double slope; /* time slope between new reference and old reference (for sanity check) */ + + bool aber_n0; /* is the update value for synchronization aberrant or not ? */ + static bool aber_min1 = false; /* keep track of whether value at sync N-1 was aberrant or not */ + static bool aber_min2 = false; /* keep track of whether value at sync N-2 was aberrant or not */ CHECK_NULL(ref); /* calculate the slope */ - if (ref->systime != 0) { - cnt_diff = (double)(count_us - ref->count_us) / (double)(TS_CPS); /* uncorrected by xtal_err */ - utc_diff = (double)((gps_time.tv_sec - timezone) - (ref->utc).tv_sec) + (1E-9 * (double)(gps_time.tv_nsec - (ref->utc).tv_nsec)); + cnt_diff = (double)(count_us - ref->count_us) / (double)(TS_CPS); /* uncorrected by xtal_err */ + utc_diff = (double)(utc.tv_sec - (ref->utc).tv_sec) + (1E-9 * (double)(utc.tv_nsec - (ref->utc).tv_nsec)); - if (!calibrating && utc_diff < 1.0) - return LGW_GPS_ERROR; - - if (cnt_diff != 0 && utc_diff != 0) { // prevent divide by zero - slope = cnt_diff/utc_diff; + /* detect aberrant points by measuring if slope limits are exceeded */ + if (utc_diff != 0) { // prevent divide by zero + slope = cnt_diff/utc_diff; + if ((slope > PLUS_10PPM) || (slope < MINUS_10PPM)) { + DEBUG_MSG("Warning: correction range exceeded\n"); + aber_n0 = true; } else { - slope = 0.0; - } - - update = (slope >= MINUS_10PPM && slope <= PLUS_10PPM); - - if (calibrating && !update && utc_diff > 1.5) { - update = true; - } else if (update) { - calibrating = false; + aber_n0 = false; } - } else { - update = true; - slope = 1.0; + DEBUG_MSG("Warning: aberrant UTC value for synchronization\n"); + aber_n0 = true; } - if (update || calibrating) { + /* watch if the 3 latest sync point were aberrant or not */ + if (aber_n0 == false) { + /* value no aberrant -> sync with smoothed slope */ + ref->systime = time(NULL); + ref->count_us = count_us; + ref->utc.tv_sec = utc.tv_sec; + ref->utc.tv_nsec = utc.tv_nsec; + ref->gps.tv_sec = gps_time.tv_sec; + ref->gps.tv_nsec = gps_time.tv_nsec; + ref->xtal_err = slope; + aber_min2 = aber_min1; + aber_min1 = aber_n0; + return LGW_GPS_SUCCESS; + } else if (aber_n0 && aber_min1 && aber_min2) { + /* 3 successive aberrant values -> sync reset (keep xtal_err) */ ref->systime = time(NULL); ref->count_us = count_us; - ref->utc.tv_sec = gps_time.tv_sec - timezone; - ref->utc.tv_nsec = gps_time.tv_nsec; - ref->gps.tv_sec = gps_time.tv_sec + leap_seconds - 315964800; + ref->utc.tv_sec = utc.tv_sec; + ref->utc.tv_nsec = utc.tv_nsec; + ref->gps.tv_sec = gps_time.tv_sec; ref->gps.tv_nsec = gps_time.tv_nsec; - ref->xtal_err = (slope >= MINUS_10PPM && slope <= PLUS_10PPM) ? slope : 1.0; + /* reset xtal_err only if the present value is out of range */ + if ((ref->xtal_err > PLUS_10PPM) || (ref->xtal_err < MINUS_10PPM)) { + ref->xtal_err = 1.0; + } + DEBUG_MSG("Warning: 3 successive aberrant sync attempts, sync reset\n"); + aber_min2 = aber_min1; + aber_min1 = aber_n0; return LGW_GPS_SUCCESS; + } else { + /* only 1 or 2 successive aberrant values -> ignore and return an error */ + aber_min2 = aber_min1; + aber_min1 = aber_n0; + return LGW_GPS_ERROR; } - return LGW_GPS_ERROR; + return LGW_GPS_SUCCESS; } - /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ int lgw_cnt2utc(struct tref ref, uint32_t count_us, struct timespec *utc) { diff --git a/libloragw/tst/test_loragw_gps.c b/libloragw/tst/test_loragw_gps.c index 994c0ad..16ea6f0 100644 --- a/libloragw/tst/test_loragw_gps.c +++ b/libloragw/tst/test_loragw_gps.c @@ -51,6 +51,8 @@ struct tref ppm_ref; /* --- PRIVATE FUNCTIONS DECLARATION ---------------------------------------- */ static void sig_handler(int sigio); +static void gps_process_sync(void); +static void gps_process_coords(void); /* -------------------------------------------------------------------------- */ /* --- PRIVATE FUNCTIONS DEFINITION ----------------------------------------- */ @@ -63,6 +65,72 @@ static void sig_handler(int sigio) { } } +static void gps_process_sync(void) { + /* variables for PPM pulse GPS synchronization */ + uint32_t ppm_tstamp; + struct timespec ppm_gps; + struct timespec ppm_utc; + + /* variables for timestamp <-> GPS time conversions */ + uint32_t x, z; + struct timespec y; + + /* get GPS time for synchronization */ + int i = lgw_gps_get(&ppm_utc, &ppm_gps, NULL, NULL); + if (i != LGW_GPS_SUCCESS) { + printf(" No valid reference GPS time available, synchronization impossible.\n"); + return; + } + + /* get timestamp for synchronization */ + i = lgw_get_trigcnt(&ppm_tstamp); + if (i != LGW_HAL_SUCCESS) { + printf(" Failed to read timestamp, synchronization impossible.\n"); + return; + } + + /* try to update synchronize time reference with the new GPS & timestamp */ + i = lgw_gps_sync(&ppm_ref, ppm_tstamp, ppm_utc, ppm_gps); + if (i != LGW_GPS_SUCCESS) { + printf(" Synchronization error.\n"); + return; + } + + /* display result */ + printf(" * Synchronization successful *\n"); + printf(" UTC reference time: %lld.%09ld\n", (long long)ppm_ref.utc.tv_sec, ppm_ref.utc.tv_nsec); + printf(" GPS reference time: %lld.%09ld\n", (long long)ppm_ref.gps.tv_sec, ppm_ref.gps.tv_nsec); + printf(" Internal counter reference value: %u\n", ppm_ref.count_us); + printf(" Clock error: %.9f\n", ppm_ref.xtal_err); + + x = ppm_tstamp + 500000; + printf(" * Test of timestamp counter <-> GPS value conversion *\n"); + printf(" Test value: %u\n", x); + lgw_cnt2gps(ppm_ref, x, &y); + printf(" Conversion to GPS: %lld.%09ld\n", (long long)y.tv_sec, y.tv_nsec); + lgw_gps2cnt(ppm_ref, y, &z); + printf(" Converted back: %u ==> %dµs\n", z, (int32_t)(z-x)); + printf(" * Test of timestamp counter <-> UTC value conversion *\n"); + printf(" Test value: %u\n", x); + lgw_cnt2utc(ppm_ref, x, &y); + printf(" Conversion to UTC: %lld.%09ld\n", (long long)y.tv_sec, y.tv_nsec); + lgw_utc2cnt(ppm_ref, y, &z); + printf(" Converted back: %u ==> %dµs\n", z, (int32_t)(z-x)); +} + +static void gps_process_coords(void) { + /* position variable */ + struct coord_s coord; + struct coord_s gpserr; + int i = lgw_gps_get(NULL, NULL, &coord, &gpserr); + + /* update gateway coordinates */ + if (i == LGW_GPS_SUCCESS) { + printf("# GPS coordinates: latitude %.5f, longitude %.5f, altitude %i m\n", coord.lat, coord.lon, coord.alt); + printf("# GPS err: latitude %.5f, longitude %.5f, altitude %i m\n", gpserr.lat, gpserr.lon, gpserr.alt); + } +} + /* -------------------------------------------------------------------------- */ /* --- MAIN FUNCTION -------------------------------------------------------- */ @@ -78,9 +146,15 @@ int main() /* serial variables */ char serial_buff[128]; /* buffer to receive GPS data */ + size_t wr_idx = 0; /* pointer to end of chars in buffer */ + int gps_tty_dev; /* file descriptor to the serial port of the GNSS module */ /* NMEA/UBX variables */ - static struct tref time_reference_gps; /* time reference used for GPS <-> timestamp conversion */ + enum gps_msg latest_msg = UNKNOWN; /* keep track of latest NMEA/UBX message parsed */ + + fd_set fds; + char delim[4] = "$"; + char *token[254]; /* configure signal handling */ sigemptyset(&sigact.sa_mask); @@ -95,9 +169,9 @@ int main() printf("*** Library version information ***\n%s\n***\n", lgw_version_info()); /* Open and configure GPS */ - i = lgw_gps_enable(&gpsdata, &source); + i = lgw_gps_enable("/dev/ttyS0", "ubx7", 0, &gps_tty_dev, 1); if (i != LGW_GPS_SUCCESS) { - printf("ERROR: IMPOSSIBLE TO ENABLE GPS\n"); + printf("ERROR: Failed to enable GPS\n"); exit(EXIT_FAILURE); } @@ -124,41 +198,88 @@ int main() memset(&ppm_ref, 0, sizeof ppm_ref); /* loop until user action */ - while ((quit_sig != 1) && (exit_sig != 1)) { - wait_ms(100); - int r = gps_read(&gpsdata, 0, 0); - if (r!= -1 && (gpsdata.status != STATUS_NO_FIX) && - (gpsdata.fix.mode == MODE_2D || gpsdata.fix.mode == MODE_3D) && - !isnan(gpsdata.fix.latitude) && - !isnan(gpsdata.fix.longitude)) { - - - uint32_t trig_tstamp; /* concentrator timestamp associated with PPM pulse */ - - i = lgw_get_trigcnt(&trig_tstamp); - lgw_gps_sync(&time_reference_gps, trig_tstamp, gpsdata.fix.time); - - printf("\n--- GPS ---\n"); - printf("Set: %lld\n", gpsdata.set); - printf("Online: %10.0f\n", gpsdata.online); - printf("Status: %d\n", gpsdata.status); - printf("Satellites Used: %d\n", gpsdata.satellites_used); - printf("Mode: %d\n", gpsdata.fix.mode); - printf("UTC time: %lld.%09ld\n", (long long)time_reference_gps.utc.tv_sec, time_reference_gps.utc.tv_nsec); - printf("GPS time: %lld.%09ld\n", (long long)time_reference_gps.gps.tv_sec, time_reference_gps.gps.tv_nsec); - printf("Latitude: %f\n", gpsdata.fix.latitude); - printf("Longitude: %f\n", gpsdata.fix.longitude); - printf("Altitude: %fm\n", gpsdata.fix.altitude); - printf("Speed: %f\n", gpsdata.fix.speed); - printf("Track: %f\n", gpsdata.fix.track); - printf("Pdop: %f\n", gpsdata.dop.pdop); + 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); + } + 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(); + } + } + } + } + } + } else { + if (r == -1) { + if (errno == EAGAIN) + continue; + else { + printf(stderr, "gpspipe: read error %s(%d)\n", strerror(errno), errno); + exit(EXIT_FAILURE); + } + } else { + exit(EXIT_SUCCESS); + } } } /* clean up before leaving */ if (exit_sig == 1) { - lgw_gps_disable(&gpsdata); + lgw_gps_disable(gps_tty_dev); lgw_stop(); } -- cgit v1.2.3