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/src/loragw_gps.c | 238 ++++++++++++++------------------------------- 1 file changed, 74 insertions(+), 164 deletions(-) (limited to 'libloragw/src') diff --git a/libloragw/src/loragw_gps.c b/libloragw/src/loragw_gps.c index c0e0ded..3aad031 100644 --- a/libloragw/src/loragw_gps.c +++ b/libloragw/src/loragw_gps.c @@ -84,6 +84,7 @@ static double gps_mlo = 0.0; /* minutes of longitude */ static char gps_olo = 0; /* orientation (E-W) of longitude */ static short gps_alt = 0; /* altitude */ static bool gps_pos_ok = false; +static bool gps_lock_ok = false; static char gps_mod = 'N'; /* GPS mode (N no fix, A autonomous, D differential) */ static short gps_sat = 0; /* number of satellites used for fix */ @@ -251,109 +252,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; +int lgw_gps_enable(struct gps_data_t *gpsdata, struct fixsource_t *source) { - /* 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 - } + unsigned int flags; + fd_set fds; + flags = WATCH_ENABLE; + flags |= WATCH_RAW; + flags |= WATCH_NMEA; + gpsd_source_spec(NULL, source); - /* get actual serial port configuration */ - i = tcgetattr(gps_tty_dev, &ttyopt); - if (i != 0) { - DEBUG_MSG("ERROR: IMPOSSIBLE TO GET TTY PORT CONFIGURATION\n"); + 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; } - /* 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"); - 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 +282,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; } @@ -457,7 +364,8 @@ 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 */ { @@ -472,11 +380,24 @@ 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 - } else { /* valid */ + 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) { + gps_time_ok = false; + gps_lock_ok = false; } - return UBX_NAV_TIMEGPS; + return UBX_NAV_TIMEUTC; } else if ((serial_buff[2] == 0x05) && (serial_buff[3] == 0x00)) { DEBUG_MSG("NOTE: UBX ACK-NAK received\n"); return IGNORED; @@ -526,7 +447,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,33 +456,32 @@ 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; + gps_lock_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_time_ok = false; + gps_lock_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", 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 @@ -587,6 +507,8 @@ 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; @@ -662,38 +584,49 @@ 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 utc, struct timespec gps_time) { + bool update = false; double cnt_diff; /* internal concentrator time difference (in seconds) */ double utc_diff; /* UTC time difference (in seconds) */ - 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 */ + double slope = 1.0; /* time slope between new reference and old reference (for sanity check) */ + static bool calibrating = true; CHECK_NULL(ref); /* calculate the slope */ - 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 (ref->systime != 0) { + + 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)); - /* 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; + if (cnt_diff != 0 && utc_diff != 0) { // prevent divide by zero + slope = cnt_diff/utc_diff; } else { - aber_n0 = false; + slope = 0.0; + } + + if (gps_lock_ok && gps_time_ok && cnt_diff > 1.5) { + update = true; } + + update = (slope >= MINUS_10PPM && slope <= PLUS_10PPM); + + if (!calibrating && utc_diff < 10.0) { + return LGW_GPS_ERROR; + } + + if (calibrating && !update && utc_diff > 1.5) { + update = true; + } else if (update) { + calibrating = false; + } + } else { - DEBUG_MSG("Warning: aberrant UTC value for synchronization\n"); - aber_n0 = true; + update = true; + slope = 0.0; } - /* watch if the 3 latest sync point were aberrant or not */ - if (aber_n0 == false) { - /* value no aberrant -> sync with smoothed slope */ + if (update || calibrating) { ref->systime = time(NULL); ref->count_us = count_us; ref->utc.tv_sec = utc.tv_sec; @@ -701,33 +634,10 @@ int lgw_gps_sync(struct tref *ref, uint32_t count_us, struct timespec utc, struc 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 = 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; - /* 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_SUCCESS; + return LGW_GPS_ERROR; } /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ -- cgit v1.2.3