diff options
Diffstat (limited to 'gnss-service/src/gnss-use-nmea.cpp')
| -rw-r--r-- | gnss-service/src/gnss-use-nmea.cpp | 88 |
1 files changed, 70 insertions, 18 deletions
diff --git a/gnss-service/src/gnss-use-nmea.cpp b/gnss-service/src/gnss-use-nmea.cpp index a817d48..9fb8472 100644 --- a/gnss-service/src/gnss-use-nmea.cpp +++ b/gnss-service/src/gnss-use-nmea.cpp @@ -32,6 +32,8 @@ #include "gnss-init.h" //GNSS data types #include "gnss.h" +//GNSS status +#include "gnss-status.h" //helper functions to dispatch the receeived data #include "globals.h" //log/trace macros @@ -66,7 +68,6 @@ #define GNSS_DELAY 0 #endif - DLT_DECLARE_CONTEXT(gContext); /** @@ -75,10 +76,13 @@ DLT_DECLARE_CONTEXT(gContext); * */ - /** Flag to terminate NMEA reader thread */ volatile int g_GNSS_NMEA_loop=1; +/** Maximum number of retries to re-open GNSS_DEVICE when select() returns an error */ +#define OPEN_RETRY_MAX 15 +/** Delay between retiers in seconds */ +#define OPEN_RETRY_DELAY 2 /** * Provide a system timestamp in milliseconds. @@ -97,6 +101,22 @@ static uint64_t gnss_get_timestamp() } } +/** + * Helper function to conventiently set GNSS status + */ +void setGNSSStatus(EGNSSStatus newStatus) +{ + static EGNSSStatus lastStatus = GNSS_STATUS_NOTAVAILABLE; + if(newStatus != lastStatus) + { + lastStatus = newStatus; + TGNSSStatus status = {0}; + status.timestamp = gnss_get_timestamp(); + status.status = newStatus; + status.validityBits = GNSSSTATUS_STATUS_VALID; + updateGNSSStatus(&status); + } +} /** * Convert GPS data from NMEA parser to TGNSSPosition struct @@ -338,8 +358,10 @@ void* loop_GNSS_NMEA_device(void* dev) int maxfd; /* maximum file desciptor used */ int linecount=0; char buf[255]; + //read failure - used to trigger restart + bool read_failure = false; + //trigger message NMEA_RESULT trigger = NMEA_GPRMC; - //gps data as returned by NMEA parser GPS_DATA gps_data; HNMEA_Init_GPS_DATA(&gps_data); @@ -349,18 +371,18 @@ void* loop_GNSS_NMEA_device(void* dev) while (g_GNSS_NMEA_loop) { int res; - struct timeval Timeout; + struct timeval timeout; /* set timeout value within input loop */ - Timeout.tv_usec = 0; /* milliseconds */ - Timeout.tv_sec = 2; /* seconds */ + timeout.tv_usec = 0; /* milliseconds */ + timeout.tv_sec = 2; /* seconds */ FD_SET(fd, &readfs); maxfd = fd+1; /* block until input becomes available */ - res = select(maxfd, &readfs, NULL, NULL, &Timeout); + res = select(maxfd, &readfs, NULL, NULL, &timeout); if (res==-1) { - //LOG_DEBUG_MSG(gContext, "select()\n"); + read_failure = true; } else if (res==0) { @@ -369,6 +391,10 @@ void* loop_GNSS_NMEA_device(void* dev) else if (FD_ISSET(fd, &readfs)) { res = read(fd,buf,255); + if (res == 0) + { + read_failure = true; + } buf[res]=0; /* set end of string, so we can printf */ linecount++; //LOG_DEBUG(gContext, "%d:%s", linecount, buf); @@ -389,6 +415,7 @@ void* loop_GNSS_NMEA_device(void* dev) #endif if (nmea_res == trigger) { + setGNSSStatus(GNSS_STATUS_AVAILABLE); uint64_t timestamp = gnss_get_timestamp() - GNSS_DELAY; TGNSSTime gnss_time = { 0 }; TGNSSPosition gnss_pos = { 0 }; @@ -402,7 +429,32 @@ void* loop_GNSS_NMEA_device(void* dev) } } } + if(read_failure) + { + //Error - try to restart device connection + setGNSSStatus(GNSS_STATUS_RESTARTING); + close(fd); + fd = -1; + int device_open_retries = 0; + while ((device_open_retries < OPEN_RETRY_MAX) && (fd < 0)) + { + device_open_retries++; + sleep(OPEN_RETRY_DELAY); + fd = open_GNSS_NMEA_device(GNSS_DEVICE, GNSS_BAUDRATE); + } + if (fd >=0) + { + read_failure = false; + } + else + { + //reopen failed: terminate thread + setGNSSStatus(GNSS_STATUS_FAILURE); + g_GNSS_NMEA_loop = 0; + } + } } + close(fd); //LOG_DEBUG_MSG(gContext, "END NMEA reading loop\n"); return NULL; } @@ -421,22 +473,22 @@ int g_fd = -1; extern bool gnssInit() { + setGNSSStatus(GNSS_STATUS_INITIALIZING); g_fd = open_GNSS_NMEA_device(GNSS_DEVICE, GNSS_BAUDRATE); - //U-blox receivers: try to activate GPGST + if (g_fd >=0) + { + //U-blox receivers: try to activate GPGST #ifdef GNSS_CHIPSET_UBLOX - char act_gst[] = "$PUBX,40,GST,0,0,0,1,0,0*5A\r\n"; - //printf("GNSS_CHIPSET == UBLOX\n"); - write(g_fd, act_gst, strlen(act_gst)); + char act_gst[] = "$PUBX,40,GST,0,0,0,1,0,0*5A\r\n"; + //printf("GNSS_CHIPSET == UBLOX\n"); + write(g_fd, act_gst, strlen(act_gst)); #endif - - if (g_fd >=0) - { - pthread_create (&g_thread, NULL, loop_GNSS_NMEA_device, &g_fd); + pthread_create(&g_thread, NULL, loop_GNSS_NMEA_device, &g_fd); return true; } else { - //perror(GNSS_DEVICE); + setGNSSStatus(GNSS_STATUS_FAILURE); return false; } } @@ -444,7 +496,7 @@ extern bool gnssInit() extern bool gnssDestroy() { g_GNSS_NMEA_loop = 0; - pthread_join (g_thread, NULL); + pthread_join(g_thread, NULL); //LOG_DEBUG_MSG(gContext, "gnssDestroy: NMEA reader thread terminated\n"); return true; } |