diff options
| -rw-r--r-- | lora_pkt_fwd/src/lora_pkt_fwd.c | 157 |
1 files changed, 30 insertions, 127 deletions
diff --git a/lora_pkt_fwd/src/lora_pkt_fwd.c b/lora_pkt_fwd/src/lora_pkt_fwd.c index c75624f..a3f8814 100644 --- a/lora_pkt_fwd/src/lora_pkt_fwd.c +++ b/lora_pkt_fwd/src/lora_pkt_fwd.c @@ -3299,140 +3299,43 @@ void thread_jit(void) { /* -------------------------------------------------------------------------- */ /* --- THREAD 4: PARSE GPS MESSAGE AND KEEP GATEWAY IN SYNC ----------------- */ -static void gps_process_sync(void) { - struct timespec gps_time; - struct timespec utc; - uint32_t trig_tstamp; /* concentrator timestamp associated with PPM pulse */ - int i = lgw_gps_get(&utc, &gps_time, NULL, NULL); - - /* get GPS time for synchronization */ - if (i != LGW_GPS_SUCCESS) { - MSG("WARNING: [gps] could not get GPS time from GPS\n"); - return; - } - - /* get timestamp captured on PPM pulse */ - pthread_mutex_lock(&mx_concent); - i = lgw_get_trigcnt(&trig_tstamp); - pthread_mutex_unlock(&mx_concent); - if (i != LGW_HAL_SUCCESS) { - MSG("WARNING: [gps] failed to read concentrator timestamp\n"); - return; - } - - /* try to update time reference with the new GPS time & timestamp */ - pthread_mutex_lock(&mx_timeref); - i = lgw_gps_sync(&time_reference_gps, trig_tstamp, utc, gps_time); - pthread_mutex_unlock(&mx_timeref); - if (i != LGW_GPS_SUCCESS) { -// MSG("WARNING: [gps] GPS out of sync, keeping previous time reference\n"); - } -} - -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 */ - pthread_mutex_lock(&mx_meas_gps); - if (i == LGW_GPS_SUCCESS) { - gps_coord_valid = true; - meas_gps_coord = coord; - meas_gps_err = gpserr; - // TODO: report other GPS statistics (typ. signal quality & integrity) - } else { - gps_coord_valid = false; - } - pthread_mutex_unlock(&mx_meas_gps); -} - void thread_gps(void) { - char serial_buff[128]; /* buffer to receive GPS data */ - enum gps_msg latest_msg; /* keep track of latest NMEA message parsed */ - memset(serial_buff, 0, sizeof serial_buff); /* initialize some variables before loop */ - fd_set fds; - char delim[4] = "$"; - char *token[254]; while (!exit_sig && !quit_sig) { - 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) { - MSG("gpspipe: select error %s(%d)\n", strerror(errno), errno); - exit(EXIT_FAILURE); - } else if (r == 0) - continue; + wait_ms(100); + int r = gps_read (&gpsdata,0,0); + int used=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)) { - /* 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)){ - latest_msg = 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(); - } - } - } - } + pthread_mutex_lock(&mx_meas_gps); + gps_coord_valid = true; + meas_gps_coord.lat = gpsdata.fix.latitude; + meas_gps_coord.lon = gpsdata.fix.longitude; + meas_gps_coord.alt = gpsdata.fix.altitude; + pthread_mutex_unlock(&mx_meas_gps); + + uint32_t trig_tstamp; /* concentrator timestamp associated with PPM pulse */ + + /* get timestamp captured on PPM pulse */ + pthread_mutex_lock(&mx_concent); + int i = lgw_get_trigcnt(&trig_tstamp); + pthread_mutex_unlock(&mx_concent); + if (i != LGW_HAL_SUCCESS) { + MSG("WARNING: [gps] failed to read concentrator timestamp\n"); + return; } + + /* try to update time reference with the new GPS time & timestamp */ + pthread_mutex_lock(&mx_timeref); + lgw_gps_sync(&time_reference_gps, trig_tstamp, gpsdata.fix.time); + pthread_mutex_unlock(&mx_timeref); } else { - if (r == -1) { - if (errno == EAGAIN) - continue; - else { - MSG(stderr, "gpspipe: read error %s(%d)\n", strerror(errno), errno); - exit(EXIT_FAILURE); - } - } else { - exit(EXIT_SUCCESS); - } + gps_coord_valid = false; } } + lgw_gps_disable(&gpsdata); MSG("\nINFO: End of GPS thread\n"); } |