Added changes for gps thread to work with the gpsd library

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");
 }