Changed gps thread to use gpspipe4.0.18

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;