align with new version of navigation, fix bug in obd2

7 files changed, 161 insertions, 30 deletions

diff --git a/src/build.sh b/src/build.sh
index e86a501..1537cc8 100755
--- a/src/build.sh
+++ b/src/build.sh
@@ -106,7 +106,7 @@ do
 		echo "-l: Build with dlt (only with -c)"
 		echo "-m: Build with commonAPI plugins (only with -c)"
 		echo "-n: Build navit"
-		echo "-t: Generate the HMI theme"
+		echo "-t: Generate the HMI theme (only with -c)"
 		echo "-w: Enable migration to the html based hmi"
 		exit 1
 	esac
diff --git a/src/hmi/gimp/gdp-theme/800x480/NavigationAppPOI.xcf b/src/hmi/gimp/gdp-theme/800x480/NavigationAppPOI.xcf
index f3cd72a..e0a2eaa 100644
--- a/src/hmi/gimp/gdp-theme/800x480/NavigationAppPOI.xcf
+++ b/src/hmi/gimp/gdp-theme/800x480/NavigationAppPOI.xcf
diff --git a/src/update_third_party.sh b/src/update_third_party.sh
index 99bbd58..01f184f 100755
--- a/src/update_third_party.sh
+++ b/src/update_third_party.sh
@@ -2,7 +2,7 @@
 
 navigation=0
 navit=0
-navigation_version='4c3e24b04f8ff1e41a94f1c1dd181ae3412c3db9'
+navigation_version='db252d95b806c7080c597a14b8011c9d8db0658d'
 positioning_version='9725fe1f553197042d6445997690d452a73490c0'
 navit_version='1e71b5fd4c0bf5ac96e5207c51db7d17057ed798'
 
diff --git a/src/vehicle-gateway/gnss.cpp b/src/vehicle-gateway/gnss.cpp
index 5cbe341..22b085c 100644
--- a/src/vehicle-gateway/gnss.cpp
+++ b/src/vehicle-gateway/gnss.cpp
@@ -170,8 +170,8 @@ void* loop_gnss_device(void* dev)
         int res;
         struct timeval timeout;
         /* set timeout value within input loop */
-        timeout.tv_usec = 0;  /* milliseconds */
-        timeout.tv_sec  = 2;  /* seconds */
+        timeout.tv_usec = 100000;  /* µseconds */
+        timeout.tv_sec  = 0;  /* seconds */
         FD_SET(fd, &readfs);
         maxfd = fd+1;
 
diff --git a/src/vehicle-gateway/obd2.cpp b/src/vehicle-gateway/obd2.cpp
index a32e1ce..3734bc1 100644
--- a/src/vehicle-gateway/obd2.cpp
+++ b/src/vehicle-gateway/obd2.cpp
@@ -71,12 +71,21 @@ included by <termios.h> */
 #define ELM_PROMPT '>'
 #define ELM_READ_LOOP 5000 //5 ms
 #define ELM_READ_TIMEOUT 1000000 //100 ms
+#define ELM_HEADER_ON "AT H1\r\n"
+#define ELM_CAN_FORMAT_OFF "AT CAF0\r\n"
+#define ELM_MONITOR_ALL "AT MA\r\n"
+#define ELM_SET_CAN_ID_MASK "AT CM"
+#define ELM_SET_CAN_ID_MASK_LENGTH 12
+#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
 
+
 static int g_obd2_fd = -1;
 static struct termios g_oldtio;
 
@@ -201,7 +210,7 @@ bool obd2_read_answer(char*& ans,size_t& length,uint64_t& timestamp)
 
 bool obd2_send_command(const char* cmd)
 {
-    if (write(g_obd2_fd,cmd,sizeof(cmd)-1))
+    if (write(g_obd2_fd,cmd,strlen(cmd)))
         return true;
     else
         return false;
@@ -258,6 +267,37 @@ bool obd2_config(uint64_t& timestamp)
     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){
+            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
@@ -292,6 +332,11 @@ bool obd2_read_engine_rpm(uint16_t& rpm,uint64_t& timestamp)
     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)
 {
     //`012F` Fuel Tank Level Input: returns 1 byte: level in %
@@ -323,3 +368,37 @@ bool obd2_read_fuel_tank_level(uint8_t& level,uint64_t& timestamp)
     }
     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;
+    size_t answer_length;
+    char filterBuffer[ELM_SET_CAN_ID_FILTER_LENGTH];
+    char maskBuffer[ELM_SET_CAN_ID_MASK_LENGTH];
+
+    sprintf(filterBuffer,ELM_SET_CAN_ID_FILTER " %x" CR_LF,filter);
+    sprintf(maskBuffer,ELM_SET_CAN_ID_MASK " %x" CR_LF,mask);
+
+    if (obd2_send_command(filterBuffer)){
+        answer=NULL;
+        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+            return false;
+        }
+    }else{
+        return false;
+    }
+    if (obd2_send_command(maskBuffer)){
+        answer=NULL;
+        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+            return false;
+        }
+    }else{
+        return false;
+    }
+    return true;
+}
diff --git a/src/vehicle-gateway/obd2.h b/src/vehicle-gateway/obd2.h
index 98fa69c..b3cd5e0 100644
--- a/src/vehicle-gateway/obd2.h
+++ b/src/vehicle-gateway/obd2.h
@@ -51,8 +51,16 @@ 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_config_can_reader(uint64_t& timestamp);
+
+bool obd2_set_filter(uint16_t filter, uint16_t mask, uint64_t& timestamp);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/vehicle-gateway/veh-gateway.cpp b/src/vehicle-gateway/veh-gateway.cpp
index a835936..4a6b0bd 100644
--- a/src/vehicle-gateway/veh-gateway.cpp
+++ b/src/vehicle-gateway/veh-gateway.cpp
@@ -62,6 +62,9 @@ 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;
@@ -72,6 +75,7 @@ typedef struct
 DLT_DECLARE_CONTEXT(gContext);
 
 bool isRunning=true;
+bool can_reader_mode;
 
 static int g_obd2_fd = -1;
 static struct termios g_oldtio;
@@ -138,16 +142,25 @@ 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");
-        return false;
-    }else{
-        LOG_DEBUG(gContext,"Engine speed: %d",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;
+    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;
+        }
     }
+
+    LOG_DEBUG(gContext,"Engine speed: %d",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;
 }
 
@@ -156,16 +169,25 @@ 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");
-        return false;
-    }else{
-        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);
-        sock_buf=tmp;
+    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;
+        }
     }
+
+    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);
+    sock_buf=tmp;
+
     return true;
 }
 
@@ -201,6 +223,9 @@ int main(int argc, char* argv[])
     char gnss_buf[MAX_GNSS_BUFFER_SIZE];
     uint64_t gnss_timestamp;
 
+    // default arguments
+    can_reader_mode=false;
+    ipaddr = (char*)IPADDR_DEFAULT;
     // arguments check
     if(argc < 3)
     {
@@ -211,10 +236,15 @@ int main(int argc, char* argv[])
     {
         modem_device_obd2 = argv[1];
         modem_device_gnss = argv[2];
-        if(argc < 4)
-            ipaddr = (char*)IPADDR_DEFAULT;
-        else
-            ipaddr = argv[3];
+        if(argc == 4)
+            can_reader_mode= argv[3];
+        else{
+            if(argc == 5)
+                ipaddr = argv[4];
+            else
+            if(argc > 5)
+                LOG_ERROR_MSG(gContext,"two many parameters");
+        }
     }
 
 
@@ -283,11 +313,25 @@ int main(int argc, char* argv[])
         return(-1);
     }
 
-    // set echo OFF
-    start = get_timestamp();
-    if(!obd2_config(stop)){
-        LOG_DEBUG(gContext,"ECHO OFF OBD2 FAILURE [DURATION = %" PRIu64 " ms]", stop-start);
-        return(-1);
+    if(can_reader_mode){
+        // can reader mode
+        start = get_timestamp();
+        if(!obd2_config_can_reader(stop)){
+            LOG_DEBUG(gContext,"ECHO OFF 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);
+            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);
+            return(-1);
+        }
     }
 
     //config the GNSS device to send given frames (TBD)