summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--lora_pkt_fwd/src/lora_pkt_fwd.c157
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");
}