GT-3206 SNS API: Extend API to provide sensor status information + a bit GNSS POC update

10 files changed, 239 insertions, 11 deletions

diff --git a/gnss-service/api/gnss-status.h b/gnss-service/api/gnss-status.h
index 21bd294..9815005 100644
--- a/gnss-service/api/gnss-status.h
+++ b/gnss-service/api/gnss-status.h
@@ -31,15 +31,15 @@ typedef enum {
     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. */
+    GNSS_STATUS_OUTOFSERVICE  = 5     /**< GNSS is temporarily not available, due to some known external condition, e.g. firmware update or switch off for antenna supervision. */
 } EGNSSStatus;
 
 /**
- * TGNSSStatus::validityBits provides information about the currently valid signals of the GNSS status struct.
+ * TGNSSStatus::validityBits provides information about the currently valid signals of the TGNSSStatus struct.
  * It is a or'ed bitmask of the EGNSSStatusValidityBits values.
  */
 typedef enum {
-    GNSSSTATUS_STATUS_VALID         = 0x00000001    /**< Validity bit for field TGNSSStatus::status. */
+    GNSS_STATUS_STATUS_VALID         = 0x00000001    /**< Validity bit for field TGNSSStatus::status. */
 } EGNSSStatusValidityBits;
 
 /**
diff --git a/gnss-service/src/gnss-use-nmea.cpp b/gnss-service/src/gnss-use-nmea.cpp
index 9fb8472..6a24462 100644
--- a/gnss-service/src/gnss-use-nmea.cpp
+++ b/gnss-service/src/gnss-use-nmea.cpp
@@ -49,9 +49,14 @@
 #include <stdlib.h>
 #include <string.h>
 #include <time.h>
+#include <sys/timeb.h>
 //the NMEA parser
 #include "hnmea.h"
 
+
+//activate this #define to print raw NMEA
+//#define NMEA_PRINT_RAW
+
 /**
  * CONFIGURATION PARAMETERS
  *
@@ -113,7 +118,7 @@ void setGNSSStatus(EGNSSStatus newStatus)
         TGNSSStatus status = {0};
         status.timestamp = gnss_get_timestamp();
         status.status = newStatus;
-        status.validityBits = GNSSSTATUS_STATUS_VALID;
+        status.validityBits = GNSS_STATUS_STATUS_VALID;
         updateGNSSStatus(&status);
     }
 }   
@@ -145,7 +150,13 @@ bool extractPosition(const GPS_DATA& gps_data, uint64_t timestamp, TGNSSPosition
         gnss_pos.altitudeMSL = gps_data.alt;
         gnss_pos.validityBits |= GNSS_POSITION_ALTITUDEMSL_VALID;
         if (gps_data.valid & GPS_DATA_GEOID)
-        {   //http://earth-info.nga.mil/GandG/wgs84/gravitymod/wgs84_180/intptWhel.html
+        {
+            //Geoid separation terminology might be difficult to understand
+            //You can cross check your NMEA output and calculation results
+            //with an online geoid calculator such as:
+            //  http://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html
+            //  http://earth-info.nga.mil/GandG/wgs84/gravitymod/wgs84_180/intptWhel.html
+            //  http://geographiclib.sourceforge.net/cgi-bin/GeoidEval
             gnss_pos.altitudeEll = gps_data.alt+gps_data.geoid;
             gnss_pos.validityBits |= GNSS_POSITION_ALTITUDEELL_VALID;
         }
@@ -241,7 +252,7 @@ bool extractTime(const GPS_DATA& gps_data, uint64_t timestamp, TGNSSTime& gnss_t
         gnss_time.hour = gps_data.time_hh;
         gnss_time.minute = gps_data.time_mm;
         gnss_time.second = gps_data.time_ss;
-        gnss_time.ms = 0;
+        gnss_time.ms = gps_data.time_ms;
         gnss_time.validityBits |= GNSS_TIME_TIME_VALID; 
     }
     if (gps_data.valid & GPS_DATA_DATE) 
@@ -398,7 +409,10 @@ void* loop_GNSS_NMEA_device(void* dev)
             buf[res]=0;             /* set end of string, so we can printf */
             linecount++;
             //LOG_DEBUG(gContext, "%d:%s", linecount, buf);
-            //printf("%"PRIu64":%s",gnss_get_timestamp(), buf);
+            #ifdef NMEA_PRINT_RAW
+            printf("%"PRIu64",0,%s",gnss_get_timestamp(), buf);
+            fflush(stdout);
+            #endif
             NMEA_RESULT nmea_res = HNMEA_Parse(buf, &gps_data);
 
             //most receivers sent GPRMC as last, but u-blox send as first: use other trigger
@@ -422,6 +436,22 @@ void* loop_GNSS_NMEA_device(void* dev)
                 if (extractTime(gps_data, timestamp, gnss_time))
                 {
                     updateGNSSTime(&gnss_time, 1);
+
+                    #ifdef NMEA_PRINT_RAW
+                    /* try to determine GNSS_DELAY assumming a well NTP-synched clock */
+                    /* http://www.ntp.org/ntpfaq/NTP-s-sw-clocks-quality.htm */
+                    struct timeb curtime;
+                    struct tm *gmttime;
+                    /* Get the current time. */
+                    ftime (&curtime);
+                    /* Convert it to local time representation. */
+                    gmttime = gmtime (&(curtime.time));
+                    printf("%"PRIu64",0,$HOSTTIME,%04d,%02d,%02d,%02d,%02d,%02d,%03d\n",
+                           gnss_get_timestamp(),
+                           gmttime->tm_year+1900, gmttime->tm_mon, gmttime->tm_mday,
+                           gmttime->tm_hour, gmttime->tm_min, gmttime->tm_sec, curtime.millitm);
+                    fflush(stdout);
+                    #endif
                 }
                 if (extractPosition(gps_data, timestamp, gnss_pos))
                 {
@@ -479,9 +509,11 @@ extern bool gnssInit()
     {
         //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";
+    char act_grs[] = "$PUBX,40,GRS,0,0,0,1,0,0*5C\r\n";
+    //printf("GNSS_CHIPSET == UBLOX\n");
+    write(g_fd, act_gst, strlen(act_gst));
+    write(g_fd, act_grs, strlen(act_grs));
 #endif
         pthread_create(&g_thread, NULL, loop_GNSS_NMEA_device, &g_fd);
         return true;
diff --git a/gnss-service/src/hnmea.cpp b/gnss-service/src/hnmea.cpp
index b059b70..d176c9e 100644
--- a/gnss-service/src/hnmea.cpp
+++ b/gnss-service/src/hnmea.cpp
@@ -48,6 +48,7 @@ void HNMEA_Init_GPS_DATA(GPS_DATA* gps_data)
     gps_data->time_hh       = -1;
     gps_data->time_mm       = -1;
     gps_data->time_ss       = -1;
+    gps_data->time_ms       = -1;
     gps_data->course        = -999.99;
     gps_data->speed         = -999.99;
     gps_data->hdop          = -999.99;
@@ -154,6 +155,7 @@ void HNMEA_Parse_GPRMC(char* line, GPS_DATA* gps_data)
                 if (strlen (field) >=6)
                 {
                     gps_data->time_ss = atoi(field+4);
+                    gps_data->time_ms = (atof(field+4)-gps_data->time_ss)*1000;
                     field[4] = '\0';
                     gps_data->time_mm = atoi(field+2);
                     field[2] = '\0';
@@ -347,6 +349,7 @@ void HNMEA_Parse_GPGGA(char* line, GPS_DATA* gps_data)
                 if (strlen (field) >=6)
                 {
                     gps_data->time_ss = atoi(field+4);
+                    gps_data->time_ms = (atof(field+4)-gps_data->time_ss)*1000;
                     field[4] = '\0';
                     gps_data->time_mm = atoi(field+2);
                     field[2] = '\0';
@@ -732,6 +735,7 @@ void HNMEA_Parse_GPGST(char* line, GPS_DATA* gps_data)
                 if (strlen (field) >=6)
                 {
                     gps_data->time_ss = atoi(field+4);
+                    gps_data->time_ms = (atof(field+4)-gps_data->time_ss)*1000;
                     field[4] = '\0';
                     gps_data->time_mm = atoi(field+2);
                     field[2] = '\0';
@@ -875,3 +879,4 @@ NMEA_RESULT HNMEA_Parse(char* line, GPS_DATA* gps_data)
     }
     return ret;
 }
+
diff --git a/gnss-service/src/hnmea.h b/gnss-service/src/hnmea.h
index b26184d..2e9a489 100644
--- a/gnss-service/src/hnmea.h
+++ b/gnss-service/src/hnmea.h
@@ -69,6 +69,7 @@ typedef struct {
     int time_hh;    //time::hour
     int time_mm;    //time::minute
     int time_ss;    //time::second
+    int time_ms;    //time::millisecond
     double course;  //course (heading) in degrees compass-like
     double speed;   //speed im m/s
     float hdop;     //hdop
@@ -86,3 +87,4 @@ void HNMEA_Init_GPS_DATA(GPS_DATA* gps_data);
 NMEA_RESULT HNMEA_Parse(char* line, GPS_DATA* gps_data);
 
 #endif //_HNMEA_H
+
diff --git a/logger/test/log-gnss-sns.cpp b/logger/test/log-gnss-sns.cpp
index 171cbd7..08611b3 100644
--- a/logger/test/log-gnss-sns.cpp
+++ b/logger/test/log-gnss-sns.cpp
@@ -91,7 +91,7 @@ static void cbPosition(const TGNSSPosition position[], uint16_t numElements)
 
 static void cbGNSSStatus(const TGNSSStatus *status)
 {
-    if (status && (status->validityBits & GNSSSTATUS_STATUS_VALID))
+    if (status && (status->validityBits & GNSS_STATUS_STATUS_VALID))
     {
         char status_string[64];
         sprintf(status_string, "#GNSS Status: %d", status->status);
diff --git a/sensors-service/api/acceleration.h b/sensors-service/api/acceleration.h
index 62764b4..f68e506 100644
--- a/sensors-service/api/acceleration.h
+++ b/sensors-service/api/acceleration.h
@@ -19,6 +19,7 @@
 #define INCLUDED_GENIVI_ACCELERATION
 
 #include "sns-meta-data.h"
+#include "sns-status.h"
 #include <stdbool.h>
 
 #ifdef __cplusplus
@@ -164,6 +165,13 @@ typedef struct {
 typedef void (*AccelerationCallback)(const TAccelerationData accelerationData[], uint16_t numElements);
 
 /**
+ * Callback type for acceleration sensor status.
+ * Use this type of callback if you want to register for acceleration sensor status updates data.
+ * @param status the acceleration sensor status
+ */
+typedef void (*AccelerationStatusCallback)(const TSensorStatus *status);
+
+/**
  * Initialization of the acceleration sensor service.
  * Must be called before using the acceleration sensor service to set up the service.
  * @return True if initialization has been successfull.
@@ -218,6 +226,30 @@ bool snsAccelerationRegisterCallback(AccelerationCallback callback);
  */
 bool snsAccelerationDeregisterCallback(AccelerationCallback callback);
 
+/**
+ * Method to get the acceleration sensor status at a specific point in time.
+ * @param status After calling the method the current acceleration sensor status is written into status
+ * @return Is true if data can be provided and false otherwise, e.g. missing initialization
+ */
+bool snsAccelerationGetStatus(TSensorStatus* status);
+
+/**
+ * Register acceleration sensor status callback.
+ * This is the recommended method for continuously monitoring the acceleration sensor status.
+ * The callback will be invoked when new acceleration sensor status data is available.
+ * @param callback The callback which should be registered.
+ * @return True if callback has been registered successfully.
+ */
+bool snsAccelerationRegisterStatusCallback(AccelerationStatusCallback callback);
+
+/**
+ * Deregister acceleration sensor status callback.
+ * After calling this method no new acceleration sensor 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 snsAccelerationDeregisterStatusCallback(AccelerationStatusCallback callback);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/sensors-service/api/gyroscope.h b/sensors-service/api/gyroscope.h
index 3425b25..18669b0 100644
--- a/sensors-service/api/gyroscope.h
+++ b/sensors-service/api/gyroscope.h
@@ -19,6 +19,7 @@
 #define INCLUDE_GENIVI_GYROSCOPE
 
 #include "sns-meta-data.h"
+#include "sns-status.h"
 #include <stdbool.h>
 
 #ifdef __cplusplus
@@ -163,6 +164,13 @@ typedef struct {
 typedef void (*GyroscopeCallback)(const TGyroscopeData gyroData[], uint16_t numElements);
 
 /**
+ * Callback type for gyroscope status.
+ * Use this type of callback if you want to register for gyroscope status updates data.
+ * @param status the gyroscope status
+ */
+typedef void (*GyroscopeStatusCallback)(const TSensorStatus *status);
+
+/**
  * Initialization of the gyroscope sensor service.
  * Must be called before using the gyroscope sensor service to set up the service.
  * @return True if initialization has been successfull.
@@ -220,6 +228,30 @@ bool snsGyroscopeRegisterCallback(GyroscopeCallback callback);
  */
 bool snsGyroscopeDeregisterCallback(GyroscopeCallback callback);
 
+/**
+ * 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 snsGyroscopeGetStatus(TSensorStatus* status);
+
+/**
+ * Register gyroscope 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 snsGyroscopeRegisterStatusCallback(GyroscopeStatusCallback callback);
+
+/**
+ * Deregister gyroscope status callback.
+ * After calling this method no new gyroscope 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 snsGyroscopeDeregisterStatusCallback(GyroscopeStatusCallback callback);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/sensors-service/api/sns-status.h b/sensors-service/api/sns-status.h
new file mode 100644
index 0000000..ec0972c
--- /dev/null
+++ b/sensors-service/api/sns-status.h
@@ -0,0 +1,61 @@
+/**************************************************************************
+ * @licence app begin@
+ *
+ * SPDX-License-Identifier: MPL-2.0
+ *
+ * \ingroup SensorsService
+ * \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_SNS_STATUS
+#define INCLUDE_GENIVI_SNS_STATUS
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Enumeration to describe the status of the GNSS receiver
+ */
+typedef enum {
+    SENSOR_STATUS_NOTAVAILABLE  = 0,    /**< Sensor is not available at all, based on configuration data. */
+    SENSOR_STATUS_INITIALIZING  = 1,    /**< Initial status when the connection to the Sensor is set up for the first time. */
+    SENSOR_STATUS_AVAILABLE     = 2,    /**< Sensor is available and running as expected. */
+    SENSOR_STATUS_RESTARTING    = 3,    /**< Sensor is restarted, i.e. due to communication loss. */
+    SENSOR_STATUS_FAILURE       = 4,    /**< Sensor is not operating properly. Restarting did not help. */
+    SENSOR_STATUS_OUTOFSERVICE  = 5     /**< Sensor is temporarily not available, due to some known external condition, vehicle bus or external ECU providing he signal being off. */
+} ESensorStatus;
+
+/**
+ * TGNSSStatus::validityBits provides information about the currently valid signals of the TSensorStatus struct.
+ * It is a or'ed bitmask of the EGNSSStatusValidityBits values.
+ */
+typedef enum {
+    SENSOR_STATUS_STATUS_VALID          = 0x00000001    /**< Validity bit for field TSensorStatus::status. */
+} ESensorStatusValidityBits;
+
+/**
+ * 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. */
+    ESensorStatus status;           /**< Status of the sensor receiver */
+    uint32_t validityBits;          /**< Bit mask indicating the validity of each corresponding value.
+                                        [bitwise or'ed @ref ESensorStatusValidityBits values].
+                                        Must be checked before usage. */
+} TSensorStatus;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //#ifndef INCLUDE_GENIVI_SNS_STATUS
diff --git a/sensors-service/api/vehicle-speed.h b/sensors-service/api/vehicle-speed.h
index ade85d9..c5897e5 100644
--- a/sensors-service/api/vehicle-speed.h
+++ b/sensors-service/api/vehicle-speed.h
@@ -19,6 +19,7 @@
 #define INCLUDED_GENIVI_VEHICLESPEED
 
 #include "sns-meta-data.h"
+#include "sns-status.h"
 #include <stdbool.h>
 
 #ifdef __cplusplus
@@ -57,6 +58,14 @@ typedef struct {
  */
 typedef void (*VehicleSpeedCallback)(const TVehicleSpeedData vehicleSpeedData[], uint16_t numElements);
 
+
+/**
+ * Callback type for vehicle speed sensor status.
+ * Use this type of callback if you want to register for vehicle speed sensor status updates data.
+ * @param status the vehicle speed sensor status
+ */
+typedef void (*VehicleSpeedStatusCallback)(const TSensorStatus *status);
+
 /**
  * Initialization of the vehicle speed sensor service.
  * Must be called before using the vehicle speed sensor service to set up the service.
@@ -102,6 +111,30 @@ bool snsVehicleSpeedRegisterCallback(VehicleSpeedCallback callback);
  */
 bool snsVehicleSpeedDeregisterCallback(VehicleSpeedCallback callback);
 
+/**
+ * Method to get the vehicle speed sensor status at a specific point in time.
+ * @param status After calling the method the current vehicle speed sensor status is written into status
+ * @return Is true if data can be provided and false otherwise, e.g. missing initialization
+ */
+bool snsVehicleSpeedGetStatus(TSensorStatus* status);
+
+/**
+ * Register vehicle speed sensor status callback.
+ * This is the recommended method for continuously monitoring the vehicle speed sensor status.
+ * The callback will be invoked when new vehicle speed sensor status data is available.
+ * @param callback The callback which should be registered.
+ * @return True if callback has been registered successfully.
+ */
+bool snsVehicleSpeedRegisterStatusCallback(VehicleSpeedStatusCallback callback);
+
+/**
+ * Deregister vehicle speed sensor status callback.
+ * After calling this method no new vehicle speed sensor 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 snsVehicleSpeedDeregisterStatusCallback(VehicleSpeedStatusCallback callback);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/sensors-service/api/wheel.h b/sensors-service/api/wheel.h
index 77ea17a..3a42d84 100644
--- a/sensors-service/api/wheel.h
+++ b/sensors-service/api/wheel.h
@@ -19,6 +19,7 @@
 #define INCLUDE_GENIVI_WHEEL
 
 #include "sns-meta-data.h"
+#include "sns-status.h"
 #include <stdbool.h>
 
 #ifdef __cplusplus
@@ -221,6 +222,13 @@ typedef struct {
 typedef void (*WheelCallback)(const TWheelData wheelData[], uint16_t numElements);
 
 /**
+ * Callback type for wheel sensor status.
+ * Use this type of callback if you want to register for wheel sensor status updates data.
+ * @param status the wheel sensor status
+ */
+typedef void (*WheelStatusCallback)(const TSensorStatus *status);
+
+/**
  * Initialization of the wheel sensor service.
  * Must be called before using the wheel sensor service to set up the service.
  * @return True if initialization has been successfull.
@@ -273,6 +281,29 @@ bool snsWheelRegisterCallback(WheelCallback callback);
  */
 bool snsWheelDeregisterCallback(WheelCallback callback);
 
+/**
+ * Method to get the wheel sensor status at a specific point in time.
+ * @param status After calling the method the current wheel sensor status is written into status
+ * @return Is true if data can be provided and false otherwise, e.g. missing initialization
+ */
+bool snsWheelGetStatus(TSensorStatus* status);
+
+/**
+ * Register wheel sensor status callback.
+ * This is the recommended method for continuously monitoring the wheel sensor status.
+ * The callback will be invoked when new wheel sensor status data is available.
+ * @param callback The callback which should be registered.
+ * @return True if callback has been registered successfully.
+ */
+bool snsWheelRegisterStatusCallback(WheelStatusCallback callback);
+
+/**
+ * Deregister wheel sensor status callback.
+ * After calling this method no new wheel sensor 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 snsWheelDeregisterStatusCallback(WheelStatusCallback callback);
 
 #ifdef __cplusplus
 }