complete vehicle gateway and test

8 files changed, 248 insertions, 61 deletions

diff --git a/src/vehicle-gateway/CMakeLists.txt b/src/vehicle-gateway/CMakeLists.txt
index fe4629b..4157899 100644
--- a/src/vehicle-gateway/CMakeLists.txt
+++ b/src/vehicle-gateway/CMakeLists.txt
@@ -37,6 +37,7 @@ set(PRJ_SRCS
     ${CMAKE_CURRENT_SOURCE_DIR}/obd2.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/hnmea.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/gnss.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/common.cpp
 )
 
 include_directories(
diff --git a/src/vehicle-gateway/common.cpp b/src/vehicle-gateway/common.cpp
new file mode 100644
index 0000000..21e30fd
--- /dev/null
+++ b/src/vehicle-gateway/common.cpp
@@ -0,0 +1,38 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \ingroup Vehicle gateway
+* \author Philippe Colliot <philippe.colliot@mpsa.com>
+*
+* \copyright Copyright (C) 2017, PSA Group
+*
+* \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@
+**************************************************************************/
+
+#include <common.h>
+#include <time.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+
+uint64_t get_timestamp()
+{
+  struct timespec time_value;
+  if (clock_gettime(CLOCK_MONOTONIC, &time_value) != -1)
+  {
+    return (time_value.tv_sec*1000 + time_value.tv_nsec/1000000);
+  }
+  else
+  {
+    return 0xFFFFFFFFFFFFFFFF;
+  }
+}
+
diff --git a/src/vehicle-gateway/common.h b/src/vehicle-gateway/common.h
new file mode 100644
index 0000000..bd9be21
--- /dev/null
+++ b/src/vehicle-gateway/common.h
@@ -0,0 +1,24 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \ingroup Vehicle gateway
+* \author Philippe Colliot <philippe.colliot@mpsa.com>
+*
+* \copyright Copyright (C) 2017, PSA Group
+*
+* \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_COMMON
+#define INCLUDE_COMMON
+#include <inttypes.h>
+
+uint64_t get_timestamp();
+
+#endif
diff --git a/src/vehicle-gateway/gnss.cpp b/src/vehicle-gateway/gnss.cpp
index faa4e10..5cbe341 100644
--- a/src/vehicle-gateway/gnss.cpp
+++ b/src/vehicle-gateway/gnss.cpp
@@ -47,6 +47,7 @@
 
 #include <gnss.h>
 #include <hnmea.h>
+#include <common.h>
 
 const char* ACTIVATE_GST = "$PUBX,40,GST,0,0,0,1,0,0*5A\r\n";
 const char* ACTIVATE_GRS = "$PUBX,40,GRS,0,0,0,1,0,0*5C\r\n";
@@ -59,6 +60,7 @@ static unsigned int g_baudrate;
 pthread_t g_thread_gnss;
 pthread_mutex_t mutex_gnss;
 char gnssBuffer[MAX_GNSS_BUFFER_SIZE];
+uint64_t gnssTimestamp;
 bool gnssDataReady;
 
 /** Flag to terminate NMEA reader thread */
@@ -201,6 +203,7 @@ void* loop_gnss_device(void* dev)
             {
                 pthread_mutex_lock(&mutex_gnss);       /* down semaphore */
                 strncpy(gnssBuffer,buf,res);
+                gnssTimestamp=get_timestamp();
                 gnssDataReady=true;
                 pthread_mutex_unlock(&mutex_gnss);       /* up semaphore */
             }
diff --git a/src/vehicle-gateway/gnss.h b/src/vehicle-gateway/gnss.h
index 99a8883..7e15271 100644
--- a/src/vehicle-gateway/gnss.h
+++ b/src/vehicle-gateway/gnss.h
@@ -34,6 +34,7 @@
 #ifndef INCLUDE_GNSS
 #define INCLUDE_GNSS
 #include <semaphore.h>
+#include <inttypes.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -44,7 +45,7 @@ extern "C" {
 extern pthread_mutex_t mutex_gnss;
 extern bool gnssDataReady;
 extern char gnssBuffer[];
-
+extern uint64_t gnssTimestamp;
 bool gnss_init(const char* gnss_device, unsigned int baudrate);
 bool gnss_destroy();
 
diff --git a/src/vehicle-gateway/obd2.cpp b/src/vehicle-gateway/obd2.cpp
index 0ded44c..1338284 100644
--- a/src/vehicle-gateway/obd2.cpp
+++ b/src/vehicle-gateway/obd2.cpp
@@ -38,18 +38,30 @@
 #include <unistd.h>
 #include <pthread.h>
 #include <stdio.h>
+#include <stdlib.h>
 #include <string.h>
 #include <time.h>
 #include <inttypes.h>
 
 #include <obd2.h>
+#include <common.h>
 
 /* baudrate settings are defined in <asm/termbits.h>, which is
 included by <termios.h> */
 
-#define OBD_PID_FUEL_TANK 0x012F
-#define OBD_PID_RPM 0x010C
-#define OBD_PID_VEH_SPEED 0x010D
+#define OBD_HEADER_LENGTH 5 //41 0C for instance
+
+#define OBD_FUEL_TANK_PID "012F\r\n"
+#define OBD_FUEL_TANK_MESSAGE_HEADER "41 2F"
+#define OBD_FUEL_TANK_MESSAGE_DATA_LENGTH 1
+#define OBD_FUEL_TANK_MESSAGE_LENGTH OBD_HEADER_LENGTH+3*OBD_FUEL_TANK_MESSAGE_DATA_LENGTH //41 2F 00
+
+#define OBD_RPM_PID "010C\r\n"
+#define OBD_RPM_MESSAGE_HEADER "41 0C"
+#define OBD_RPM_MESSAGE_DATA_LENGTH 2
+#define OBD_RPM_MESSAGE_LENGTH OBD_HEADER_LENGTH+3*OBD_RPM_MESSAGE_DATA_LENGTH //41 0C 00 00
+
+#define OBD_VEH_SPEED_PID "010D\r\n"
 
 #define ELM_RESET_ALL "AT Z\r\n"
 #define ELM_GET_ID "AT I\r\n"
@@ -140,7 +152,7 @@ int obd2_open_device(char* obd2_device, unsigned int baudrate)
    return fd;
 }
 
-bool obd2_read_answer(char*& ans,size_t* length)
+bool obd2_read_answer(char*& ans,size_t& length,uint64_t& timestamp)
 { //ans is allocated dynamically
     bool isRead=false;
     char buf=EOS;
@@ -165,7 +177,7 @@ bool obd2_read_answer(char*& ans,size_t* length)
                         isRead=true;
                         *(tmp+buf_length)=buf;
                         ans = tmp;
-                        *length=buf_length;
+                        length=buf_length;
                     }
                     else{
                         if(buf==CR)
@@ -180,6 +192,7 @@ bool obd2_read_answer(char*& ans,size_t* length)
         timeout+=ELM_READ_LOOP;
     }while((isRead==false)&&(timeout<ELM_READ_TIMEOUT));
 
+    timestamp=get_timestamp();
     return isRead;
 }
 
@@ -204,5 +217,84 @@ bool obd2_init(char* obd2_device, unsigned int baudrate)
     return retval;
 }
 
+bool obd2_reset()
+{
+    char* answer;
+    size_t answer_length;
+    uint64_t timestamp;
+    if (obd2_send_command(ELM_RESET_ALL)){
+        answer=NULL;
+        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+            return false;
+        }
+    }else{
+        return false;
+    }
+    return true;
+}
 
+bool obd2_read_engine_rpm(uint16_t& rpm,uint64_t& timestamp)
+{
+    //`010C` Engine RPM: returns 2 bytes (A,B): RPM [1/min] = ((A*256)+B)/4
+    char* answer;
+    char header[OBD_HEADER_LENGTH+1];
+    char value[OBD_RPM_MESSAGE_DATA_LENGTH*2+1];
+    size_t answer_length;
+    if (obd2_send_command(OBD_RPM_PID)){
+        answer=NULL;
+        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+            return false;
+        }
+    }else{
+        return false;
+    }
+    if(answer_length!=OBD_RPM_MESSAGE_LENGTH){
+        return false;
+    }else{
+        strncpy(header,answer,OBD_HEADER_LENGTH);
+        header[OBD_HEADER_LENGTH]=EOS;
+        if(strcmp(header,OBD_RPM_MESSAGE_HEADER)!=0){
+            return false;
+        }else{
+            value[0]=answer[OBD_HEADER_LENGTH+1];
+            value[1]=answer[OBD_HEADER_LENGTH+2];
+            value[2]=answer[OBD_HEADER_LENGTH+4];
+            value[3]=answer[OBD_HEADER_LENGTH+5];
+            value[4]=EOS;
+            rpm=atoi(value)/4;
+        }
+    }
+    return true;
+}
 
+bool obd2_read_fuel_tank_level(uint8_t& level,uint64_t& timestamp)
+{
+    //`012F` Fuel Tank Level Input: returns 1 byte: level in %
+    char* answer;
+    char header[OBD_HEADER_LENGTH+1];
+    char value[OBD_FUEL_TANK_MESSAGE_DATA_LENGTH*2+1];
+    size_t answer_length;
+    if (obd2_send_command(OBD_FUEL_TANK_PID)){
+        answer=NULL;
+        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+            return false;
+        }
+    }else{
+        return false;
+    }
+    if(answer_length!=OBD_FUEL_TANK_MESSAGE_LENGTH){
+        return false;
+    }else{
+        strncpy(header,answer,OBD_HEADER_LENGTH);
+        header[OBD_HEADER_LENGTH]=EOS;
+        if(strcmp(header,OBD_FUEL_TANK_MESSAGE_HEADER)!=0){
+            return false;
+        }else{
+            value[0]=answer[OBD_HEADER_LENGTH+1];
+            value[1]=answer[OBD_HEADER_LENGTH+2];
+            value[3]=EOS;
+            level=atoi(value);
+        }
+    }
+    return true;
+}
diff --git a/src/vehicle-gateway/obd2.h b/src/vehicle-gateway/obd2.h
index 8c43560..278e29a 100644
--- a/src/vehicle-gateway/obd2.h
+++ b/src/vehicle-gateway/obd2.h
@@ -43,11 +43,13 @@ extern "C" {
 #include <math.h>
 #include <sys/types.h>
 
-bool obd2_read_answer(char*& ans, size_t* length);
+bool obd2_init(char* obd2_device, unsigned int baudrate);
 
-bool obd2_send_command(const char* cmd);
+bool obd2_reset();
 
-bool obd2_init(char* obd2_device, unsigned int baudrate);
+bool obd2_read_engine_rpm(uint16_t& rpm, uint64_t &timestamp);
+
+bool obd2_read_fuel_tank_level(uint8_t& level,uint64_t& timestamp);
 
 #ifdef __cplusplus
 }
diff --git a/src/vehicle-gateway/veh-gateway.cpp b/src/vehicle-gateway/veh-gateway.cpp
index 9c4674e..3e0ac33 100644
--- a/src/vehicle-gateway/veh-gateway.cpp
+++ b/src/vehicle-gateway/veh-gateway.cpp
@@ -3,10 +3,10 @@
 *
 * SPDX-License-Identifier: MPL-2.0
 *
-* \ingroup LogReplayer
-* \author Marco Residori <marco.residori@xse.de>
+* \ingroup Vehicle gateway
+* \author Philippe Colliot <philippe.colliot@mpsa.com>
 *
-* \copyright Copyright (C) 2013, XS Embedded GmbH
+* \copyright Copyright (C) 2017, PSA Group
 * 
 * \license
 * This Source Code Form is subject to the terms of the
@@ -32,6 +32,7 @@
 #include <semaphore.h>
 #include <termios.h>
 
+#include <common.h>
 #include <obd2.h>
 #include <gnss.h>
 
@@ -43,15 +44,11 @@
 #define PORT2 9931   //port used for sensor data
 #define PORT3 9932   //port used for vehicle data
 const char * IPADDR_DEFAULT = "127.0.0.1";
-const char * GNS_PREFIX = "GVGNS";
-const char * SNS_PREFIX = "GVSNS";
-const char * VEH_PREFIX = "GVVEH";
 
+#define SCAN_LOOP_TIME 100000 //100 ms
 #define MAXDELTA 1000  //max value to avoid overflow
 
 #define BAUDRATE_OBD2 B38400
-#define ELM_RESET_ALL "AT Z\r\n"
-#define ELM_GET_ID "AT I\r\n"
 
 #define NMEA_TOKEN ","
 #define NMEA_RMC_STATUS 2
@@ -79,20 +76,9 @@ bool isRunning=true;
 static int g_obd2_fd = -1;
 static struct termios g_oldtio;
 
-uint64_t get_timestamp()
-{
-  struct timespec time_value;
-  if (clock_gettime(CLOCK_MONOTONIC, &time_value) != -1)
-  {
-    return (time_value.tv_sec*1000 + time_value.tv_nsec/1000000);
-  }
-  else
-  {
-    return 0xFFFFFFFFFFFFFFFF;
-  }
-}
-
-bool get_geolocation(char*& sock_buf,char* buffer)
+//scan the buffer, if nmea status is OK (so there's a valid location)
+//allocate the memory and compose the frame into sock_buf
+bool get_geolocation(char*& sock_buf,char* buffer,const uint64_t timestamp)
 {
     geocoordinate3D_t geolocation;
     char* tmp = new char[BUFLEN];
@@ -139,11 +125,56 @@ bool get_geolocation(char*& sock_buf,char* buffer)
     LOG_DEBUG(gContext,"Lat: %f Lon: %f Alt: %f\n",geolocation.latitude,geolocation.longitude,geolocation.altitude);
 
     //compose frame data: TIMESTAMP,0$GVGNSP,TIMESTAMP,LAT,LON,ALT,0X07
-    sprintf(tmp,"%d,%s,%d,%.6f,%.6f,%.6f,0x07",1000,"0$GVGNSP",1000,geolocation.latitude,geolocation.longitude,geolocation.altitude);
+    sprintf(tmp,"%d,%s,%d,%.6f,%.6f,%.6f,0x07",timestamp,"0$GVGNSP",timestamp,geolocation.latitude,geolocation.longitude,geolocation.altitude);
     sock_buf=tmp;
     return retval;
 }
 
+bool get_engine_speed(char*& sock_buf)
+{
+    uint16_t rpm;
+    uint64_t timestamp;
+    char* tmp = new char[BUFLEN];
+    if (obd2_read_engine_rpm(rpm,timestamp)!=true){
+        LOG_ERROR_MSG(gContext,"Read engine rpm failed\n");
+        return false;
+    }else{
+        LOG_DEBUG(gContext,"Engine speed: %d\n",rpm);
+
+        //compose frame data: TIMESTAMP,0$GVVEHENGSPEED,TIMESTAMP,RPM,0X01
+        sprintf(tmp,"%d,%s,%d,%d,0x01",timestamp,"0$GVVEHENGSPEED",timestamp,rpm);
+        sock_buf=tmp;
+    }
+    return true;
+}
+
+bool get_fuel_tank_level(char*& sock_buf)
+{
+    uint8_t fuel_level;
+    uint64_t timestamp;
+    char* tmp = new char[BUFLEN];
+    if (obd2_read_fuel_tank_level(fuel_level,timestamp)!=true){
+        LOG_ERROR_MSG(gContext,"Read fuel tank level failed\n");
+        return false;
+    }else{
+        LOG_DEBUG(gContext,"Fuel tank level: %d\%\n",fuel_level);
+
+        //compose frame data: TIMESTAMP,0$GVVEHFUELLEVEL,TIMESTAMP,LEVEL,0X01
+        sprintf(tmp,"%d,%s,%d,%d,0x01",timestamp,"0$GVVEHFUELLEVEL",timestamp,fuel_level);
+        sock_buf=tmp;
+    }
+    return true;
+}
+
+bool get_vehicle_speed(char*& sock_buf)
+{
+    return false;
+}
+
+//GVVEHFUELLEVEL
+//GVVEHFUELCONS
+//GVVEHTOTALODO
+
 void sighandler(int sig)
 {
   LOG_INFO_MSG(gContext,"Signal received");
@@ -157,20 +188,15 @@ int main(int argc, char* argv[])
     socklen_t slen = sizeof(si_other);
     int sock;
     char* sock_buf;
-    char msgId[MSGIDLEN];
     char * ipaddr = 0;
 
     // OBD and GNSS devices
     bool result;
     uint64_t start, stop;
-    char* answer;
-    size_t answer_length;
     char * modem_device_obd2 = 0;
     char * modem_device_gnss = 0;
-    char* gnssprefix = (char*)GNS_PREFIX;
-    char* snsprefix = (char*)SNS_PREFIX;
-    char* vehprefix = (char*)VEH_PREFIX;
     char gnss_buf[MAX_GNSS_BUFFER_SIZE];
+    uint64_t gnss_timestamp;
 
     // arguments check
     if(argc < 3)
@@ -248,40 +274,29 @@ int main(int argc, char* argv[])
     }
 
     // reset the OBD2 device
-    if (obd2_send_command(ELM_RESET_ALL)){
-        answer=NULL;
-        if(obd2_read_answer(answer,&answer_length)!=true){
-            LOG_ERROR_MSG(gContext,"RESET OBD2 FAILURE\n");
-            return(-1);
-        }
-    }else{
+    if(!obd2_reset()){
         LOG_ERROR_MSG(gContext,"RESET OBD2 FAILURE\n");
         return(-1);
     }
 
     //config the GNSS device to send given frames (TBD)
 
-    bool snsDataReady=false;
-    bool vehDataReady=false;
     //main loop
     do{
-        if (obd2_send_command(ELM_GET_ID)!=true){
-            LOG_DEBUG(gContext,"WRITE OBD2 FAILURE ON COMMAND: %s\n",ELM_GET_ID);
-            isRunning=false;
-        }
 
         //GNSS: list of supported message IDs
         //char* gnssstr = "GVGNSP,GVGNSC,GVGNSAC,GVGNSSAT";
+        //for the moment only GVGNSP is managed
         pthread_mutex_lock(&mutex_gnss);       /* down semaphore */
         if(gnssDataReady)
         {
             gnssDataReady=false;
             strcpy(gnss_buf,gnssBuffer); //get the gnss buffer (end of string is added in gnss code)
+            gnss_timestamp=gnssTimestamp; //get the gnss timestamp
             pthread_mutex_unlock(&mutex_gnss);       /* up semaphore */
-            if(get_geolocation(sock_buf,gnss_buf))
+            if(get_geolocation(sock_buf,gnss_buf,gnss_timestamp))
             {
                 LOG_DEBUG(gContext,"Sending Packet to %s:%d\n",ipaddr,PORT1);
-                LOG_DEBUG(gContext,"MsgID:%s\n", gnssprefix);
                 LOG_DEBUG(gContext,"Len:%d\n", (int)strlen(sock_buf));
                 LOG_DEBUG(gContext,"Data:%s\n", sock_buf);
 
@@ -299,11 +314,9 @@ int main(int argc, char* argv[])
         //SNS: list of supported message IDs
         //char* snsstr = "GVVEHSP,GVGYRO,GVGYROCONF,GVDRVDIR,GVODO,GVWHTK,GVWHTKCONF";
         //char* snsstr = "GVSNSVEHSP,GVSNSGYRO,GVSNSWHTK"; //subset currently supported for new log format
-        if(snsDataReady)
-        {
-            snsDataReady=false;
+        //for the moment no ID managed
+        if(get_vehicle_speed(sock_buf)){
             LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT2);
-            LOG_DEBUG(gContext,"MsgID:%s", msgId);
             LOG_DEBUG(gContext,"Len:%d", (int)strlen(sock_buf));
             LOG_DEBUG(gContext,"Data:%s", sock_buf);
 
@@ -314,13 +327,26 @@ int main(int argc, char* argv[])
                 return EXIT_FAILURE;
             }
         }
+
         //VHL: list of supported message IDs
         //char* vhlstr = "GVVEHVER,GVVEHENGSPEED,GVVEHFUELLEVEL,GVVEHFUELCONS,GVVEHTOTALODO";
-        if(vehDataReady)
+        //for the moment GVVEHENGSPEED, GVVEHFUELLEVEL managed
+        if(get_engine_speed(sock_buf))
+        {
+            LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT3);
+            LOG_DEBUG(gContext,"Len:%d", (int)strlen(sock_buf));
+            LOG_DEBUG(gContext,"Data:%s", sock_buf);
+
+            si_other.sin_port = htons(PORT3);
+            if(sendto(sock, sock_buf, strlen(sock_buf)+1, 0, (struct sockaddr *)&si_other, slen) == -1)
+            {
+                LOG_ERROR_MSG(gContext,"sendto() failed!");
+                return EXIT_FAILURE;
+            }
+        }
+        if(get_fuel_tank_level(sock_buf))
         {
-            vehDataReady=false;
             LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT3);
-            LOG_DEBUG(gContext,"MsgID:%s", msgId);
             LOG_DEBUG(gContext,"Len:%d", (int)strlen(sock_buf));
             LOG_DEBUG(gContext,"Data:%s", sock_buf);
 
@@ -332,7 +358,7 @@ int main(int argc, char* argv[])
             }
         }
 
-        sleep(1);
+        usleep(SCAN_LOOP_TIME);
     } while(isRunning);
 
     LOG_INFO_MSG(gContext,"Shutting down Vehicle gateway...");