diff options
| -rw-r--r-- | lora_pkt_fwd/src/lora_pkt_fwd.c | 174 |
1 files changed, 139 insertions, 35 deletions
diff --git a/lora_pkt_fwd/src/lora_pkt_fwd.c b/lora_pkt_fwd/src/lora_pkt_fwd.c index 33d1a87..c66ed81 100644 --- a/lora_pkt_fwd/src/lora_pkt_fwd.c +++ b/lora_pkt_fwd/src/lora_pkt_fwd.c @@ -206,8 +206,8 @@ static double xtal_correct = 1.0; /* GPS configuration and synchronization */ static bool use_gps = false; /* Use the GPSD stream */ static bool gps_enabled = false; /* is GPS enabled on that gateway ? */ -static struct gps_data_t gpsdata; -static struct fixsource_t source; +static char gps_tty_path[64] = "\0"; /* path of the TTY port GPS is connected on */ +static int gps_tty_fd = -1; /* file descriptor of the GPS TTY port */ /* GPS time reference */ static pthread_mutex_t mx_timeref = PTHREAD_MUTEX_INITIALIZER; /* control access to GPS time reference */ static bool gps_ref_valid; /* is GPS reference acceptable (ie. not too old) */ @@ -1703,7 +1703,7 @@ int main(int argc, char** argv) /* Start GPS a.s.a.p., to allow it to lock */ if (use_gps == true) { - int i = lgw_gps_enable(&gpsdata, &source); + int i = lgw_gps_enable(gps_tty_path, "ubx7", 0, &gps_tty_fd, 1); if (i != LGW_GPS_SUCCESS) { printf("WARNING: [main] impossible to open for GPS sync (Check GPSD)\n"); gps_enabled = false; @@ -2093,7 +2093,7 @@ int main(int argc, char** argv) if (gps_enabled == true) { pthread_cancel(thrid_gps); /* don't wait for GPS thread */ - i = lgw_gps_disable(&gpsdata); + i = lgw_gps_disable(gps_tty_fd); if (i == LGW_HAL_SUCCESS) { MSG("INFO: GPS closed successfully\n"); } else { @@ -3524,47 +3524,153 @@ 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) { - while (!exit_sig && !quit_sig) { - 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)) { + /* serial variables */ + char serial_buff[128]; /* buffer to receive GPS data */ + size_t wr_idx = 0; /* pointer to end of chars in buffer */ - 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); + /* variables for PPM pulse GPS synchronization */ + enum gps_msg latest_msg; /* keep track of latest NMEA message parsed */ + + /* initialize some variables before loop */ + memset(serial_buff, 0, sizeof serial_buff); + + while (!exit_sig && !quit_sig) { + size_t rd_idx = 0; + size_t frame_end_idx = 0; - uint32_t trig_tstamp; /* concentrator timestamp associated with PPM pulse */ + /* blocking non-canonical read on serial port */ + ssize_t nb_char = read(gps_tty_fd, serial_buff + wr_idx, LGW_GPS_MIN_MSG_SIZE); + if (nb_char <= 0) { + MSG_DEBUG(DEBUG_PKT_FWD, "WARNING: [gps] read() returned value %zd\n", nb_char); + continue; + } + wr_idx += (size_t)nb_char; + + /******************************************* + * Scan buffer for UBX/NMEA sync chars and * + * attempt to decode frame if one is found * + *******************************************/ + while (rd_idx < wr_idx) { + size_t frame_size = 0; + + /* Scan buffer for UBX sync char */ + if (serial_buff[rd_idx] == (char)LGW_GPS_UBX_SYNC_CHAR) { + + /*********************** + * Found UBX sync char * + ***********************/ + latest_msg = lgw_parse_ubx(&serial_buff[rd_idx], (wr_idx - rd_idx), &frame_size); + + if (frame_size > 0) { + if (latest_msg == INCOMPLETE) { + /* UBX header found but frame appears to be missing bytes */ + frame_size = 0; + } else if (latest_msg == INVALID) { + /* message header received but message appears to be corrupted */ + MSG("WARNING: [gps] could not get a valid message from GPS (no time)\n"); + frame_size = 0; + } else if (latest_msg == UBX_NAV_TIMEGPS) { + gps_process_sync(); + } + } + } else if (serial_buff[rd_idx] == (char)LGW_GPS_NMEA_SYNC_CHAR) { + /************************ + * Found NMEA sync char * + ************************/ + /* scan for NMEA end marker (LF = 0x0a) */ + char* nmea_end_ptr = memchr(&serial_buff[rd_idx],(int)0x0a, (wr_idx - rd_idx)); + + if(nmea_end_ptr) { + /* found end marker */ + frame_size = nmea_end_ptr - &serial_buff[rd_idx] + 1; + latest_msg = lgw_parse_nmea(&serial_buff[rd_idx], frame_size); + + 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(); + } + } + } - /* 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; + if (frame_size > 0) { + /* At this point message is a checksum verified frame + we're processed or ignored. Remove frame from buffer */ + rd_idx += frame_size; + frame_end_idx = rd_idx; + } else { + rd_idx++; } + } /* ...for(rd_idx = 0... */ - /* 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); + if (frame_end_idx) { + /* Frames have been processed. Remove bytes to end of last processed frame */ + memcpy(serial_buff, &serial_buff[frame_end_idx], wr_idx - frame_end_idx); + wr_idx -= frame_end_idx; + } /* ...for(rd_idx = 0... */ - } else { - gps_coord_valid = false; + /* Prevent buffer overflow */ + if ((sizeof(serial_buff) - wr_idx) < LGW_GPS_MIN_MSG_SIZE) { + memcpy(serial_buff, &serial_buff[LGW_GPS_MIN_MSG_SIZE], wr_idx - LGW_GPS_MIN_MSG_SIZE); + wr_idx -= LGW_GPS_MIN_MSG_SIZE; } } - lgw_gps_disable(&gpsdata); MSG("\nINFO: End of GPS thread\n"); } @@ -3703,7 +3809,6 @@ void thread_spectralscan(void) { uint16_t retry_limit = 1500; int freq_idx; uint32_t freq_nb; - int old_min = -1; uint64_t freq_reg; uint32_t freq; uint8_t read_burst[RSSI_RANGE*2]; @@ -3934,7 +4039,6 @@ void thread_spectralscan(void) { } strcat(array_string, "\"]"); json_object_set_value(root_object, "results", json_parse_string(array_string)); - old_min = min; pthread_mutex_lock(&mx_scan_report); scan_ready = true; |