GT-3206 GNSS API: Extend API to provide sensor status information

5 files changed, 231 insertions, 25 deletions

diff --git a/gnss-service/api/gnss-meta-data.h b/gnss-service/api/gnss-meta-data.h
index e2eebd7..44fd518 100644
--- a/gnss-service/api/gnss-meta-data.h
+++ b/gnss-service/api/gnss-meta-data.h
@@ -16,8 +16,8 @@
  **************************************************************************/
 
 
-#ifndef INCLUDE_GNSS_METADATA
-#define INCLUDE_GNSS_METADATA
+#ifndef INCLUDE_GENIVI_GNSS_METADATA
+#define INCLUDE_GENIVI_GNSS_METADATA
 
 #include <stdint.h>
 
@@ -73,4 +73,4 @@ bool gnssGetMetaData(TGnssMetaData *data);
 }
 #endif
 
-#endif /* INCLUDE_GNSS_METADATA */
+#endif /* INCLUDE_GENIVI_GNSS_METADATA */
diff --git a/gnss-service/api/gnss-status.h b/gnss-service/api/gnss-status.h
new file mode 100644
index 0000000..21bd294
--- /dev/null
+++ b/gnss-service/api/gnss-status.h
@@ -0,0 +1,90 @@
+/**************************************************************************
+ * @licence app begin@
+ *
+ * SPDX-License-Identifier: MPL-2.0
+ *
+ * \ingroup GNSSService
+ * \brief Compliance Level: Abstract Component
+ * \copyright Copyright (C) 2012, BMW Car IT GmbH, Continental Automotive GmbH, PCA Peugeot Citroën, XS Embedded GmbH
+ * 
+ * \license
+ * This Source Code Form is subject to the terms of the
+ * Mozilla Public License, v. 2.0. If a copy of the MPL was not distributed with
+ * this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * @licence end@
+ **************************************************************************/
+
+#ifndef INCLUDE_GENIVI_GNSS_STATUS
+#define INCLUDE_GENIVI_GNSS_STATUS
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Enumeration to describe the status of the GNSS receiver
+ */
+typedef enum {
+    GNSS_STATUS_NOTAVAILABLE  = 0,    /**< GNSS is not available at all, based on configuration data. */    
+    GNSS_STATUS_INITIALIZING  = 1,    /**< Initial status when the connection to the GNSS is set up for the first time. */
+    GNSS_STATUS_AVAILABLE     = 2,    /**< GNSS is available and running as expected. */
+    GNSS_STATUS_RESTARTING    = 3,    /**< GNSS is restarted, i.e. due to communication loss. */
+    GNSS_STATUS_FAILURE       = 4,    /**< GNSS is not operating properly. Restarting did not help. */
+    GNSS_STATUS_OUTOFSERVICE  = 5     /**< GNSS is temporarily not available, due to some known external condition, e.g. switch off for antenna supervision. */
+} EGNSSStatus;
+
+/**
+ * TGNSSStatus::validityBits provides information about the currently valid signals of the GNSS status struct.
+ * It is a or'ed bitmask of the EGNSSStatusValidityBits values.
+ */
+typedef enum {
+    GNSSSTATUS_STATUS_VALID         = 0x00000001    /**< Validity bit for field TGNSSStatus::status. */
+} EGNSSStatusValidityBits;
+
+/**
+ * Container for GNSS status information
+ */
+typedef struct {
+    uint64_t timestamp;             /**< Timestamp of the GNSS status transition [ms].
+                                         All sensor/GNSS timestamps must be based on the same time source. */
+    EGNSSStatus status;             /**< Status of the GNSS receiver */
+    uint32_t validityBits;          /**< Bit mask indicating the validity of each corresponding value.
+                                        [bitwise or'ed @ref EGNSSStatusValidityBits values].
+                                        Must be checked before usage. */
+} TGNSSStatus;    
+
+/**
+* Callback type for gnss status.
+* Use this type of callback if you want to register for gnss status updates data.
+* @param status the gnss status
+*/
+typedef void (*GNSSStatusCallback)(const TGNSSStatus *status);
+/**
+* Method to get the gyroscope status at a specific point in time.
+* @param status After calling the method the current gyroscope status is written into status
+* @return Is true if data can be provided and false otherwise, e.g. missing initialization
+*/
+bool gnssGetStatus(TGNSSStatus* status);
+/**
+* Register gnss status callback.
+* This is the recommended method for continuously monitoring the gyroscope status.
+* The callback will be invoked when new gyroscope status data is available.
+* @param callback The callback which should be registered.
+* @return True if callback has been registered successfully.
+*/
+bool gnssRegisterStatusCallback(GNSSStatusCallback callback);
+/**
+* Deregister gnss status callback.
+* After calling this method no new gnss status updates will be delivered to the client.
+* @param callback The callback which should be deregistered.
+* @return True if callback has been deregistered successfully.
+*/
+bool gnssDeregisterStatusCallback(GNSSStatusCallback callback);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //#ifndef INCLUDE_GENIVI_GNSS_STATUS
diff --git a/gnss-service/src/globals.h b/gnss-service/src/globals.h
index 7d0dc00..8436902 100644
--- a/gnss-service/src/globals.h
+++ b/gnss-service/src/globals.h
@@ -26,6 +26,7 @@
 #include "gnss-init.h"
 #include "gnss.h"
 #include "gnss-meta-data.h"
+#include "gnss-status.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -36,6 +37,7 @@ extern const TGnssMetaData gMetaData;
 void updateGNSSTime(const TGNSSTime time[], uint16_t numElements);
 void updateGNSSPosition(const TGNSSPosition position[], uint16_t numElements);
 void updateGNSSSatelliteDetail(const TGNSSSatelliteDetail satelliteDetail[], uint16_t numElements);
+void updateGNSSStatus(const TGNSSStatus* status);
 
 #ifdef __cplusplus
 }
diff --git a/gnss-service/src/gnss-impl.c b/gnss-service/src/gnss-impl.c
index 2fbb74b..fcb48cb 100644
--- a/gnss-service/src/gnss-impl.c
+++ b/gnss-service/src/gnss-impl.c
@@ -18,6 +18,8 @@
 
 #include "globals.h"
 #include "gnss.h"
+#include "gnss-status.h"
+
 
 static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the callbacks
 static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
@@ -31,7 +33,8 @@ static volatile GNSSPositionCallback cbPosition = 0;
 static TGNSSTime gTime = {0};
 static volatile GNSSTimeCallback cbTime = 0;
 
-
+static TGNSSStatus gStatus = {0};
+static volatile GNSSStatusCallback cbStatus = 0;
 
 
 bool gnssRegisterSatelliteDetailCallback(GNSSSatelliteDetailCallback callback)
@@ -172,9 +175,53 @@ bool gnssGetTime(TGNSSTime* time)
     return retval;
 }
 
+bool gnssRegisterStatusCallback(GNSSStatusCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    //only if valid callback and not already registered
+    if(callback && !cbStatus)
+    {
+        cbStatus = callback;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+
+bool gnssDeregisterStatusCallback(GNSSStatusCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    if((cbStatus == callback) && callback)
+    {
+        cbStatus = 0;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+bool gnssGetStatus(TGNSSStatus* status)
+{
+    bool retval = false;
+    if(status)
+    {
+        pthread_mutex_lock(&mutexData);
+        *status = gStatus;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+    return retval;
+}
+
 
 void updateGNSSTime(const TGNSSTime time[], uint16_t numElements)
-//void updateGNSSTime(const TGNSSTime* gnss_time)
 {
     if (time != NULL && numElements > 0)
     {
@@ -190,9 +237,7 @@ void updateGNSSTime(const TGNSSTime time[], uint16_t numElements)
     }
 }
 
-
 void updateGNSSPosition(const TGNSSPosition position[], uint16_t numElements)
-//void updateGNSSPosition(const TGNSSPosition* gnss_pos)
 {
     if (position != NULL && numElements > 0)
     {
@@ -223,3 +268,20 @@ void updateGNSSSatelliteDetail(const TGNSSSatelliteDetail satelliteDetail[], uin
         pthread_mutex_unlock(&mutexCb);
     }
 }
+
+
+void updateGNSSStatus(const TGNSSStatus* status)
+{
+    if (status)
+    {
+        pthread_mutex_lock(&mutexData);
+        gStatus = *status;
+        pthread_mutex_unlock(&mutexData);
+        pthread_mutex_lock(&mutexCb);
+        if (cbStatus)
+        {
+            cbStatus(status);
+        }
+        pthread_mutex_unlock(&mutexCb);
+    }
+}
\ No newline at end of file
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;
 }