extend vehicle gateway

6 files changed, 334 insertions, 129 deletions

diff --git a/src/vehicle-gateway/can.h b/src/vehicle-gateway/can.h
new file mode 100644
index 0000000..2536e76
--- /dev/null
+++ b/src/vehicle-gateway/can.h
@@ -0,0 +1,53 @@
+/**************************************************************************
+* @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_CAN
+#define INCLUDE_CAN
+
+#define CAN_MESSAGE_FILTER 0x123
+#define CAN_MESSAGE_MASK 0x7FF
+#define CAN_MESSAGE_MAX_DATA_LENGTH 8
+//index is the index in the data frame, format is the length (ex: engine speed is 1758, index is 4 and format is 4
+//Engine speed frame: 20881758330040FFFF22
+// => 0x1758*0.125 = 5976*0.125 = 747 RPM
+#define CAN_MESSAGE_ENGINE_SPEED_ID_AND_DATA_SIZE "2088"
+#define CAN_MESSAGE_ENGINE_SPEED_INDEX 4
+#define CAN_MESSAGE_ENGINE_SPEED_FORMAT 4
+//Fuel level frame: 61267321013C0000
+// => 0x3C*0.5 = 60*0.5 = 30l
+#define CAN_MESSAGE_FUEL_LEVEL_ID_AND_DATA_SIZE "6126"
+#define CAN_MESSAGE_FUEL_LEVEL_INDEX 10
+#define CAN_MESSAGE_FUEL_LEVEL_FORMAT 2
+//Wheel ticks frame: 50D700091508EC8E90
+//rear left 0915 rear right 08EC  filter 7FFF error bit 15
+#define CAN_MESSAGE_WHEEL_TICK_ID_AND_DATA_SIZE "50D7"
+#define CAN_MESSAGE_RL_WHEEL_TICK_INDEX 6
+#define CAN_MESSAGE_RR_WHEEL_TICK_INDEX 10
+#define CAN_MESSAGE_WHEEL_TICK_FORMAT 4
+
+#define CAN_ID_AND_DATA_SIZE_LENGTH 4
+
+typedef enum {
+    MESSAGE_ENGINE_SPEED,
+    MESSAGE_WHEEL_TICK,
+    MESSAGE_FUEL_LEVEL,
+    MESSAGE_VEHICLE_SPEED,
+    NO_MESSAGE
+} can_message_id_t;
+
+#endif
diff --git a/src/vehicle-gateway/common.h b/src/vehicle-gateway/common.h
index bd9be21..cfc8041 100644
--- a/src/vehicle-gateway/common.h
+++ b/src/vehicle-gateway/common.h
@@ -19,6 +19,11 @@
 #define INCLUDE_COMMON
 #include <inttypes.h>
 
+#define CR '\r'
+#define CR_LF "\r\n"
+#define EOS '\0'
+#define SPACE ' '
+
 uint64_t get_timestamp();
 
 #endif
diff --git a/src/vehicle-gateway/obd2.cpp b/src/vehicle-gateway/obd2.cpp
index 3734bc1..d9dfe96 100644
--- a/src/vehicle-gateway/obd2.cpp
+++ b/src/vehicle-gateway/obd2.cpp
@@ -45,6 +45,7 @@
 
 #include <obd2.h>
 #include <common.h>
+#include <can.h>
 
 /* baudrate settings are defined in <asm/termbits.h>, which is
 included by <termios.h> */
@@ -53,10 +54,10 @@ included by <termios.h> */
 
 #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_FUEL_LEVEL_PID "012F\r\n"
+#define OBD_FUEL_LEVEL_MESSAGE_HEADER "41 2F"
+#define OBD_FUEL_LEVEL_MESSAGE_DATA_LENGTH 1
+#define OBD_FUEL_LEVEL_MESSAGE_LENGTH OBD_HEADER_LENGTH+3*OBD_FUEL_LEVEL_MESSAGE_DATA_LENGTH //41 2F 00
 
 #define OBD_RPM_PID "010C\r\n"
 #define OBD_RPM_MESSAGE_HEADER "41 0C"
@@ -79,10 +80,6 @@ included by <termios.h> */
 #define ELM_SET_CAN_ID_FILTER "AT CF"
 #define ELM_SET_CAN_ID_FILTER_LENGTH 12
 
-#define CR '\r'
-#define CR_LF "\r\n"
-#define EOS '\0'
-#define SPACE ' '
 #define BUFFER_MAX_LENGTH 512
 
 
@@ -143,7 +140,7 @@ int obd2_open_device(char* obd2_device, unsigned int baudrate)
    newtio.c_cc[VKILL]    = 0;     /* @ */
    newtio.c_cc[VEOF]     = 4;     /* Ctrl-d */
    newtio.c_cc[VTIME]    = 0;     /* inter-character timer unused */
-   newtio.c_cc[VMIN]     = 1;     /* blocking read until 1 character arrives */
+   newtio.c_cc[VMIN]     = 0;     /* non blocking read */
    newtio.c_cc[VSWTC]    = 0;     /* '\0' */
    newtio.c_cc[VSTART]   = 0;     /* Ctrl-q */
    newtio.c_cc[VSTOP]    = 0;     /* Ctrl-s */
@@ -164,9 +161,10 @@ int obd2_open_device(char* obd2_device, unsigned int baudrate)
    return fd;
 }
 
-bool obd2_read_answer(char*& ans,size_t& length,uint64_t& timestamp)
-{ //ans is allocated dynamically
+bool obd2_read(char*& data,size_t& length,uint64_t& timestamp)
+{ //data is allocated dynamically
     bool isRead=false;
+    bool isBufferOverflow=false;
     char buf=EOS;
     ssize_t read_result;
     size_t buf_length=0;
@@ -183,18 +181,20 @@ bool obd2_read_answer(char*& ans,size_t& length,uint64_t& timestamp)
                 if(buf_length>BUFFER_MAX_LENGTH)
                 {
                     printf("%s\n","buffer overflow");
+                    isBufferOverflow=true;
+                    delete tmp; //free the buffer
                     break;
                 }else{
                     if(buf==ELM_PROMPT){
                         isRead=true;
                         *(tmp+buf_length)=buf;
-                        ans = tmp;
+                        data = tmp; //copy the pointer, so it'll be freed into the caller
                         length=buf_length;
                     }
                     else{
                         if(buf==CR)
                             buf=SPACE;
-                        *(tmp+buf_length)=buf;
+                        *(tmp+buf_length)=buf; //push back char
                     }
                     buf_length++;
                 }
@@ -202,7 +202,7 @@ bool obd2_read_answer(char*& ans,size_t& length,uint64_t& timestamp)
         }
         usleep(ELM_READ_LOOP);
         timeout+=ELM_READ_LOOP;
-    }while((isRead==false)&&(timeout<ELM_READ_TIMEOUT));
+    }while((isRead==false)&&(timeout<ELM_READ_TIMEOUT)&&(isBufferOverflow==false));
 
     timestamp=get_timestamp();
     return isRead;
@@ -235,8 +235,10 @@ bool obd2_reset(uint64_t& timestamp)
     size_t answer_length;
     if (obd2_send_command(ELM_RESET_ALL)){
         answer=NULL;
-        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+        if(obd2_read(answer,answer_length,timestamp)!=true){
             return false;
+        }else{
+            delete answer;
         }
     }else{
         return false;
@@ -250,47 +252,20 @@ bool obd2_config(uint64_t& timestamp)
     size_t answer_length;
     if (obd2_send_command(ELM_ECHO_OFF)){
         answer=NULL;
-        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+        if(obd2_read(answer,answer_length,timestamp)!=true){
             return false;
+        }else{
+            delete answer;
         }
     }else{
         return false;
     }
     if (obd2_send_command(OBD_GET_PID_LIST)){
         answer=NULL;
-        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
-            return false;
-        }
-    }else{
-        return false;
-    }
-    return true;
-}
-
-bool obd2_config_can_reader(uint64_t& timestamp)
-{
-    char* answer;
-    size_t answer_length;
-    if (obd2_send_command(ELM_HEADER_ON)){
-        answer=NULL;
-        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
-            return false;
-        }
-    }else{
-        return false;
-    }
-    if (obd2_send_command(ELM_CAN_FORMAT_OFF)){
-        answer=NULL;
-        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
-            return false;
-        }
-    }else{
-        return false;
-    }
-    if (obd2_send_command(ELM_MONITOR_ALL)){
-        answer=NULL;
-        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+        if(obd2_read(answer,answer_length,timestamp)!=true){
             return false;
+        }else{
+            delete answer;
         }
     }else{
         return false;
@@ -307,18 +282,20 @@ bool obd2_read_engine_rpm(uint16_t& rpm,uint64_t& timestamp)
     size_t answer_length;
     if (obd2_send_command(OBD_RPM_PID)){
         answer=NULL;
-        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+        if(obd2_read(answer,answer_length,timestamp)!=true){
             return false;
         }
     }else{
         return false;
     }
     if(answer_length!=OBD_RPM_MESSAGE_LENGTH){
+        delete answer;
         return false;
     }else{
         strncpy(header,answer,OBD_HEADER_LENGTH);
         header[OBD_HEADER_LENGTH]=EOS;
         if(strcmp(header,OBD_RPM_MESSAGE_HEADER)!=0){
+            delete answer;
             return false;
         }else{
             value[0]=answer[OBD_HEADER_LENGTH+1];
@@ -327,53 +304,47 @@ bool obd2_read_engine_rpm(uint16_t& rpm,uint64_t& timestamp)
             value[3]=answer[OBD_HEADER_LENGTH+5];
             value[4]=EOS;
             rpm=atoi(value)/4;
+            delete answer;
         }
     }
     return true;
 }
 
-bool can_read_engine_rpm(uint16_t& rpm,uint64_t& timestamp)
-{
-    return false;
-}
-
-bool obd2_read_fuel_tank_level(uint8_t& level,uint64_t& timestamp)
+bool obd2_read_fuel_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];
+    char value[OBD_FUEL_LEVEL_MESSAGE_DATA_LENGTH*2+1];
     size_t answer_length;
-    if (obd2_send_command(OBD_FUEL_TANK_PID)){
+    if (obd2_send_command(OBD_FUEL_LEVEL_PID)){
         answer=NULL;
-        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+        if(obd2_read(answer,answer_length,timestamp)!=true){
             return false;
         }
     }else{
         return false;
     }
-    if(answer_length!=OBD_FUEL_TANK_MESSAGE_LENGTH){
+    if(answer_length!=OBD_FUEL_LEVEL_MESSAGE_LENGTH){
+        delete answer;
         return false;
     }else{
         strncpy(header,answer,OBD_HEADER_LENGTH);
         header[OBD_HEADER_LENGTH]=EOS;
-        if(strcmp(header,OBD_FUEL_TANK_MESSAGE_HEADER)!=0){
+        if(strcmp(header,OBD_FUEL_LEVEL_MESSAGE_HEADER)!=0){
+            delete answer;
             return false;
         }else{
             value[0]=answer[OBD_HEADER_LENGTH+1];
             value[1]=answer[OBD_HEADER_LENGTH+2];
             value[3]=EOS;
             level=atoi(value);
+            delete answer;
         }
     }
     return true;
 }
 
-bool can_read_fuel_tank_level(uint8_t& level,uint64_t& timestamp)
-{
-    return false;
-}
-
 bool obd2_set_filter(uint16_t filter,uint16_t mask,uint64_t& timestamp)
 {
     char* answer;
@@ -386,19 +357,123 @@ bool obd2_set_filter(uint16_t filter,uint16_t mask,uint64_t& timestamp)
 
     if (obd2_send_command(filterBuffer)){
         answer=NULL;
-        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+        if(obd2_read(answer,answer_length,timestamp)!=true){
             return false;
+        }else{
+            delete answer;
         }
     }else{
         return false;
     }
     if (obd2_send_command(maskBuffer)){
         answer=NULL;
-        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+        if(obd2_read(answer,answer_length,timestamp)!=true){
+            return false;
+        }else{
+            delete answer;
+        }
+    }else{
+        return false;
+    }
+    return true;
+}
+
+bool obd2_config_can_reader(uint64_t& timestamp)
+{
+    char* answer;
+    size_t answer_length;
+    if (obd2_send_command(ELM_HEADER_ON)){
+        answer=NULL;
+        if(obd2_read(answer,answer_length,timestamp)!=true){
+            return false;
+        }else{
+            delete answer;
+        }
+    }else{
+        return false;
+    }
+    if (obd2_send_command(ELM_CAN_FORMAT_OFF)){
+        answer=NULL;
+        if(obd2_read(answer,answer_length,timestamp)!=true){
+            return false;
+        }else{
+            delete answer;
+        }
+    }else{
+        return false;
+    }
+    if (obd2_send_command(ELM_MONITOR_ALL)){
+        answer=NULL;
+        if(obd2_read(answer,answer_length,timestamp)!=true){
             return false;
+        }else{
+            delete answer;
         }
     }else{
         return false;
     }
     return true;
 }
+
+can_message_id_t can_read(char*& data,uint64_t& timestamp)
+{
+    bool isRead=false;
+    bool isBufferOverflow=false;
+    can_message_id_t ret=NO_MESSAGE;
+    char buf=EOS;
+    ssize_t read_result;
+    size_t buf_length=0;
+    useconds_t timeout=0;
+    char* tmp = new char[BUFFER_MAX_LENGTH];
+    do{
+        read_result=read(g_obd2_fd,&buf,1);
+        if(read_result==(-1))
+            isRead=false;
+        else{
+            if(read_result>0)
+            {
+                timeout=0; //data received so reset the time out
+                if(buf_length>BUFFER_MAX_LENGTH)
+                {
+                    printf("%s\n","buffer overflow");
+                    isBufferOverflow=true;
+                    delete tmp; //free the buffer
+                    break;
+                }else{
+                    if(buf==CR){
+                        isRead=true;
+                        *(tmp+buf_length)=EOS;
+                        timestamp=get_timestamp();
+                        // analyze content now
+                        if (strncmp(tmp,CAN_MESSAGE_ENGINE_SPEED_ID_AND_DATA_SIZE,CAN_ID_AND_DATA_SIZE_LENGTH)==0)
+                        {
+                            data=tmp; //copy the pointer, so it'll be freed into the caller
+                            ret=MESSAGE_ENGINE_SPEED;
+                        }else{
+                            if (strncmp(tmp,CAN_MESSAGE_FUEL_LEVEL_ID_AND_DATA_SIZE,CAN_ID_AND_DATA_SIZE_LENGTH)==0)
+                            {
+                                data=tmp; //copy the pointer, so it'll be freed into the caller
+                                ret=MESSAGE_FUEL_LEVEL;
+                            }else{
+                                if (strncmp(tmp,CAN_MESSAGE_WHEEL_TICK_ID_AND_DATA_SIZE,CAN_ID_AND_DATA_SIZE_LENGTH)==0)
+                                {
+                                    data=tmp; //copy the pointer, so it'll be freed into the caller
+                                    ret=MESSAGE_WHEEL_TICK;
+                                }else{
+                                    delete tmp; //free the buffer
+                                }
+                            }
+                        }
+                    }else{
+                        *(tmp+buf_length)=buf; //push back char
+                    }
+                    buf_length++;
+                }
+            }
+        }
+        usleep(ELM_READ_LOOP);
+        timeout+=ELM_READ_LOOP;
+    }while((isRead==false)&&(timeout<ELM_READ_TIMEOUT)&&(isBufferOverflow==false));
+    return ret;
+}
+
diff --git a/src/vehicle-gateway/obd2.h b/src/vehicle-gateway/obd2.h
index b3cd5e0..871e68a 100644
--- a/src/vehicle-gateway/obd2.h
+++ b/src/vehicle-gateway/obd2.h
@@ -42,6 +42,7 @@ extern "C" {
 #include <stdint.h>
 #include <math.h>
 #include <sys/types.h>
+#include <can.h>
 
 bool obd2_init(char* obd2_device, unsigned int baudrate);
 
@@ -51,16 +52,14 @@ bool obd2_config(uint64_t& timestamp);
 
 bool obd2_read_engine_rpm(uint16_t& rpm, uint64_t &timestamp);
 
-bool can_read_engine_rpm(uint16_t& rpm, uint64_t &timestamp);
-
-bool obd2_read_fuel_tank_level(uint8_t& level,uint64_t& timestamp);
-
-bool can_read_fuel_tank_level(uint8_t& level,uint64_t& timestamp);
+bool obd2_read_fuel_level(uint8_t& level,uint64_t& timestamp);
 
 bool obd2_config_can_reader(uint64_t& timestamp);
 
 bool obd2_set_filter(uint16_t filter, uint16_t mask, uint64_t& timestamp);
 
+can_message_id_t can_read(char*& data, uint64_t &timestamp);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/vehicle-gateway/veh-gateway.cpp b/src/vehicle-gateway/veh-gateway.cpp
index 4a6b0bd..5956495 100644
--- a/src/vehicle-gateway/veh-gateway.cpp
+++ b/src/vehicle-gateway/veh-gateway.cpp
@@ -31,18 +31,20 @@
 #include <pthread.h>
 #include <semaphore.h>
 #include <termios.h>
+#include <iostream>
 
 #include <common.h>
 #include <obd2.h>
 #include <gnss.h>
+#include <can.h>
 
 #include <log.h>
 
 #define MSGIDLEN 20
 #define BUFLEN 256
-#define PORT1 9930   //port used for GNSS data
-#define PORT2 9931   //port used for sensor data
-#define PORT3 9932   //port used for vehicle data
+#define PORT_GNSS 9930   //port used for GNSS data
+#define PORT_SENSOR 9931   //port used for sensor data
+#define PORT_VEHICLE 9932   //port used for vehicle data
 const char * IPADDR_DEFAULT = "127.0.0.1";
 
 #define SCAN_LOOP_TIME 100000 //100 ms
@@ -62,9 +64,6 @@ const char * IPADDR_DEFAULT = "127.0.0.1";
 #define NMEA_SOUTH "S"
 #define BAUDRATE_GNSS B38400
 
-#define CAN_MESSAGE_FILTER 0x123
-#define CAN_MESSAGE_MASK 0x7FF
-
 typedef struct
 {
     double latitude;
@@ -74,8 +73,7 @@ typedef struct
 
 DLT_DECLARE_CONTEXT(gContext);
 
-bool isRunning=true;
-bool can_reader_mode;
+static bool isRunning=true;
 
 static int g_obd2_fd = -1;
 static struct termios g_oldtio;
@@ -137,22 +135,15 @@ bool get_geolocation(char*& sock_buf,char* buffer,const uint64_t timestamp)
     return retval;
 }
 
-bool get_engine_speed(char*& sock_buf)
+bool get_obd_engine_speed(char*& sock_buf)
 {
     uint16_t rpm;
     uint64_t timestamp;
     char* tmp = new char[BUFLEN];
 
-    if(can_reader_mode){
-        if (can_read_engine_rpm(rpm,timestamp)!=true){
-            LOG_ERROR_MSG(gContext,"Read engine rpm failed");
-            return false;
-        }
-    }else{
-        if (obd2_read_engine_rpm(rpm,timestamp)!=true){
-            LOG_ERROR_MSG(gContext,"Read engine rpm failed");
-            return false;
-        }
+   if (obd2_read_engine_rpm(rpm,timestamp)!=true){
+        LOG_ERROR_MSG(gContext,"Read engine rpm failed");
+        return false;
     }
 
     LOG_DEBUG(gContext,"Engine speed: %d",rpm);
@@ -164,25 +155,18 @@ bool get_engine_speed(char*& sock_buf)
     return true;
 }
 
-bool get_fuel_tank_level(char*& sock_buf)
+bool get_obd_fuel_level(char*& sock_buf)
 {
     uint8_t fuel_level;
     uint64_t timestamp;
     char* tmp = new char[BUFLEN];
 
-    if(can_reader_mode){
-        if (can_read_fuel_tank_level(fuel_level,timestamp)!=true){
-            LOG_ERROR_MSG(gContext,"Read fuel tank level failed");
-            return false;
-        }
-    }else{
-        if (obd2_read_fuel_tank_level(fuel_level,timestamp)!=true){
-            LOG_ERROR_MSG(gContext,"Read fuel tank level failed");
-            return false;
-        }
+    if (obd2_read_fuel_level(fuel_level,timestamp)!=true){
+        LOG_ERROR_MSG(gContext,"Read fuel tank level failed");
+        return false;
     }
 
-    LOG_DEBUG(gContext,"Fuel tank level: %d\%",fuel_level);
+    LOG_DEBUG(gContext,"Fuel tank level: %d",fuel_level);
 
     //compose frame data: TIMESTAMP,0$GVVEHFUELLEVEL,TIMESTAMP,LEVEL,0X01
     sprintf(tmp,"%d,%s,%d,%d,0x01",timestamp,"0$GVVEHFUELLEVEL",timestamp,fuel_level);
@@ -191,12 +175,16 @@ bool get_fuel_tank_level(char*& sock_buf)
     return true;
 }
 
-bool get_vehicle_speed(char*& sock_buf)
+bool get_obd_vehicle_speed(char*& sock_buf)
+{
+    return false;
+}
+
+bool get_obd_wheel_tick(char*& sock_buf)
 {
     return false;
 }
 
-//GVVEHFUELLEVEL
 //GVVEHFUELCONS
 //GVVEHTOTALODO
 
@@ -212,20 +200,32 @@ int main(int argc, char* argv[])
     struct sockaddr_in si_other;
     socklen_t slen = sizeof(si_other);
     int sock;
-    char* sock_buf;
     char * ipaddr = 0;
 
     // OBD and GNSS devices
     bool result;
-    uint64_t start, stop;
+    uint64_t start, stop, timestamp;
     char * modem_device_obd2 = 0;
     char * modem_device_gnss = 0;
     char gnss_buf[MAX_GNSS_BUFFER_SIZE];
     uint64_t gnss_timestamp;
 
+    // CAN reader
+    bool can_reader_mode;
+    can_message_id_t can_message_id=NO_MESSAGE;
+    int can_message_data_size;
+
     // default arguments
     can_reader_mode=false;
     ipaddr = (char*)IPADDR_DEFAULT;
+
+    // DLT init and start banner
+    DLT_REGISTER_APP("GTWY", "VEH-GATEWAY");
+    DLT_REGISTER_CONTEXT(gContext,"EMBD", "Global Context");
+
+    LOG_INFO_MSG(gContext,"------------------------------------------------");
+    LOG_INFO_MSG(gContext,"VEH GATEWAY STARTED");
+    LOG_INFO_MSG(gContext,"------------------------------------------------");
     // arguments check
     if(argc < 3)
     {
@@ -247,14 +247,8 @@ int main(int argc, char* argv[])
         }
     }
 
+    LOG_INFO(gContext,"CAN reader mode set to: %d",can_reader_mode);
 
-    // DLT init and start banner
-    DLT_REGISTER_APP("GTWY", "VEH-GATEWAY");
-    DLT_REGISTER_CONTEXT(gContext,"EMBD", "Global Context");
-
-    LOG_INFO_MSG(gContext,"------------------------------------------------");
-    LOG_INFO_MSG(gContext,"VEH GATEWAY STARTED");
-    LOG_INFO_MSG(gContext,"------------------------------------------------");
 
     // socket initialization
     signal(SIGTERM, sighandler);
@@ -317,19 +311,19 @@ int main(int argc, char* argv[])
         // can reader mode
         start = get_timestamp();
         if(!obd2_config_can_reader(stop)){
-            LOG_DEBUG(gContext,"ECHO OFF OBD2 FAILURE [DURATION = %" PRIu64 " ms]", stop-start);
+            LOG_DEBUG(gContext,"CAN READER MODE OBD2 FAILURE [DURATION = %" PRIu64 " ms]", stop-start);
             return(-1);
         }
         start = get_timestamp();
         if(!obd2_set_filter(CAN_MESSAGE_FILTER,CAN_MESSAGE_MASK,stop)){
-            LOG_DEBUG(gContext,"ECHO OFF OBD2 FAILURE [DURATION = %" PRIu64 " ms]", stop-start);
+            LOG_DEBUG(gContext,"SET FILTER OBD2 FAILURE [DURATION = %" PRIu64 " ms]", stop-start);
             return(-1);
         }
     }else{
         // config standard OBD2 with AT command
         start = get_timestamp();
         if(!obd2_config(stop)){
-            LOG_DEBUG(gContext,"ECHO OFF OBD2 FAILURE [DURATION = %" PRIu64 " ms]", stop-start);
+            LOG_DEBUG(gContext,"STANDARD MODE OBD2 FAILURE [DURATION = %" PRIu64 " ms]", stop-start);
             return(-1);
         }
     }
@@ -338,7 +332,7 @@ int main(int argc, char* argv[])
 
     //main loop
     do{
-
+        char* sock_buf;
         //GNSS: list of supported message IDs
         //char* gnssstr = "GVGNSP,GVGNSC,GVGNSAC,GVGNSSAT";
         //for the moment only GVGNSP is managed
@@ -351,11 +345,11 @@ int main(int argc, char* argv[])
             pthread_mutex_unlock(&mutex_gnss);       /* up semaphore */
             if(get_geolocation(sock_buf,gnss_buf,gnss_timestamp))
             {
-                LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT1);
+                LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT_GNSS);
                 LOG_DEBUG(gContext,"Len:%d", (int)strlen(sock_buf));
                 LOG_DEBUG(gContext,"Data:%s", sock_buf);
 
-                si_other.sin_port = htons(PORT1);
+                si_other.sin_port = htons(PORT_GNSS);
                 if(sendto(sock, sock_buf, strlen(sock_buf)+1, 0, (struct sockaddr *)&si_other, slen) == -1)
                 {
                     LOG_ERROR_MSG(gContext,"sendto() failed!");
@@ -366,51 +360,130 @@ int main(int argc, char* argv[])
             pthread_mutex_unlock(&mutex_gnss);       /* up semaphore */
         }
 
+        if(can_reader_mode)
+        {
+            char* can_message;
+            can_message_id=can_read(can_message,timestamp);
+            can_message_data_size=0;
+            if(can_message_id!=NO_MESSAGE)
+                sock_buf = new char[BUFLEN];
+            uint16_t engine_speed;
+            uint8_t fuel_level;
+            uint16_t rear_left, rear_right;
+            char dump[CAN_MESSAGE_MAX_DATA_LENGTH*2];
+            switch (can_message_id) {
+            //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
+            //for the moment no ID managed
+            //VHL: list of supported message IDs
+            //char* vhlstr = "GVVEHVER,GVVEHENGSPEED,GVVEHFUELLEVEL,GVVEHFUELCONS,GVVEHTOTALODO";
+            //for the moment GVVEHENGSPEED, GVVEHFUELLEVEL managed
+            case MESSAGE_WHEEL_TICK:
+                strncpy(dump,can_message+CAN_MESSAGE_RL_WHEEL_TICK_INDEX,CAN_MESSAGE_WHEEL_TICK_FORMAT);
+                dump[CAN_MESSAGE_WHEEL_TICK_FORMAT]=EOS;
+                can_message_data_size=sscanf(dump,"%"SCNx16,&rear_left);
+                //todo manage error bit
+                strncpy(dump,can_message+CAN_MESSAGE_RR_WHEEL_TICK_INDEX,CAN_MESSAGE_WHEEL_TICK_FORMAT);
+                dump[CAN_MESSAGE_WHEEL_TICK_FORMAT]=EOS;
+                can_message_data_size=sscanf(dump,"%"SCNx16,&rear_right);
+                //todo manage error bit
+                //compose frame data: TIMESTAMP,0$GVSNSWHE,TIMESTAMP,RR,RL,0,0,0,0,0,statusBits,measurementInterval,0X03
+                sprintf(sock_buf,"%d,%s,%d,%f,%f,0,0,0,0,0,0,0,0,0x01",timestamp,"0$GVSNSWHE",timestamp,(float)rear_left,(float)rear_right);
+                break;
+            case MESSAGE_ENGINE_SPEED:
+                strncpy(dump,can_message+CAN_MESSAGE_ENGINE_SPEED_INDEX,CAN_MESSAGE_ENGINE_SPEED_FORMAT);
+                dump[CAN_MESSAGE_ENGINE_SPEED_FORMAT]=EOS;
+                can_message_data_size=sscanf(dump,"%"SCNx16,&engine_speed);
+                engine_speed >>=3;
+                LOG_DEBUG(gContext,"Engine speed: %d RPM",engine_speed);
+
+                //compose frame data: TIMESTAMP,0$GVVEHENGSPEED,TIMESTAMP,RPM,0X01
+                sprintf(sock_buf,"%d,%s,%d,%d,0x01",timestamp,"0$GVVEHENGSPEED",timestamp,engine_speed);
+                break;
+            case MESSAGE_FUEL_LEVEL:
+                strncpy(dump,can_message+CAN_MESSAGE_FUEL_LEVEL_INDEX,CAN_MESSAGE_FUEL_LEVEL_FORMAT);
+                dump[CAN_MESSAGE_FUEL_LEVEL_FORMAT]=EOS;
+                can_message_data_size=sscanf(dump,"%"SCNx8,&fuel_level);
+                fuel_level >>=1;
+                LOG_DEBUG(gContext,"Fuel level: %d L",fuel_level);
+
+                //compose frame data: TIMESTAMP,0$GVVEHFUELLEVEL,TIMESTAMP,LEVEL,0X01
+                sprintf(sock_buf,"%d,%s,%d,%d,0x01",timestamp,"0$GVVEHFUELLEVEL",timestamp,fuel_level);
+                break;
+                break;
+            default:
+                break;
+            }
+            if(can_message_id!=NO_MESSAGE)
+                delete can_message;
+        }
+
         //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
         //for the moment no ID managed
-        if(get_vehicle_speed(sock_buf)){
-            LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT2);
+        if((!can_reader_mode && get_obd_vehicle_speed(sock_buf)) || (can_message_id==MESSAGE_VEHICLE_SPEED)){
+            LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT_SENSOR);
+            LOG_DEBUG(gContext,"Len:%d", (int)strlen(sock_buf));
+            LOG_DEBUG(gContext,"Data:%s", sock_buf);
+
+            si_other.sin_port = htons(PORT_SENSOR);
+            if(sendto(sock, sock_buf, strlen(sock_buf)+1, 0, (struct sockaddr *)&si_other, slen) == -1)
+            {
+                LOG_ERROR_MSG(gContext,"sendto() failed!");
+                delete sock_buf;
+                return EXIT_FAILURE;
+            }
+            delete sock_buf;
+        }
+        if((!can_reader_mode && get_obd_wheel_tick(sock_buf)) || (can_message_id==MESSAGE_WHEEL_TICK)){
+            LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT_SENSOR);
             LOG_DEBUG(gContext,"Len:%d", (int)strlen(sock_buf));
             LOG_DEBUG(gContext,"Data:%s", sock_buf);
 
-            si_other.sin_port = htons(PORT2);
+            si_other.sin_port = htons(PORT_SENSOR);
             if(sendto(sock, sock_buf, strlen(sock_buf)+1, 0, (struct sockaddr *)&si_other, slen) == -1)
             {
                 LOG_ERROR_MSG(gContext,"sendto() failed!");
+                delete sock_buf;
                 return EXIT_FAILURE;
             }
+            delete sock_buf;
         }
 
         //VHL: list of supported message IDs
         //char* vhlstr = "GVVEHVER,GVVEHENGSPEED,GVVEHFUELLEVEL,GVVEHFUELCONS,GVVEHTOTALODO";
         //for the moment GVVEHENGSPEED, GVVEHFUELLEVEL managed
-        if(get_engine_speed(sock_buf))
+        if((!can_reader_mode && get_obd_engine_speed(sock_buf)) || (can_message_id==MESSAGE_ENGINE_SPEED))
         {
-            LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT3);
+            LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT_VEHICLE);
             LOG_DEBUG(gContext,"Len:%d", (int)strlen(sock_buf));
             LOG_DEBUG(gContext,"Data:%s", sock_buf);
 
-            si_other.sin_port = htons(PORT3);
+            si_other.sin_port = htons(PORT_VEHICLE);
             if(sendto(sock, sock_buf, strlen(sock_buf)+1, 0, (struct sockaddr *)&si_other, slen) == -1)
             {
                 LOG_ERROR_MSG(gContext,"sendto() failed!");
+                delete sock_buf;
                 return EXIT_FAILURE;
             }
+            delete sock_buf;
         }
-        if(get_fuel_tank_level(sock_buf))
+        if((!can_reader_mode && get_obd_fuel_level(sock_buf)) || (can_message_id==MESSAGE_FUEL_LEVEL))
         {
-            LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT3);
+            LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT_VEHICLE);
             LOG_DEBUG(gContext,"Len:%d", (int)strlen(sock_buf));
             LOG_DEBUG(gContext,"Data:%s", sock_buf);
 
-            si_other.sin_port = htons(PORT3);
+            si_other.sin_port = htons(PORT_VEHICLE);
             if(sendto(sock, sock_buf, strlen(sock_buf)+1, 0, (struct sockaddr *)&si_other, slen) == -1)
             {
                 LOG_ERROR_MSG(gContext,"sendto() failed!");
+                delete sock_buf;
                 return EXIT_FAILURE;
             }
+            delete sock_buf;
         }
 
         usleep(SCAN_LOOP_TIME);
diff --git a/test/script/dashboard.bmp b/test/script/dashboard.bmp
new file mode 100644
index 0000000..01540f1
--- /dev/null
+++ b/test/script/dashboard.bmp