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/src/loragw_gps.c | 191 +++++++++++++++++++++++++++------------------ 1 file changed, 117 insertions(+), 74 deletions(-) (limited to 'libloragw/src/loragw_gps.c') 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) { -- cgit v1.2.3