some improvements of veh gateway

5 files changed, 54 insertions, 69 deletions

diff --git a/src/vehicle-gateway/can.h b/src/vehicle-gateway/can.h
index 2536e76..bb2fc06 100644
--- a/src/vehicle-gateway/can.h
+++ b/src/vehicle-gateway/can.h
@@ -19,8 +19,9 @@
 #ifndef INCLUDE_CAN
 #define INCLUDE_CAN
 
-#define CAN_MESSAGE_FILTER 0x123
-#define CAN_MESSAGE_MASK 0x7FF
+#define CAN_PROTOCOL_11_BIT_ID_500KBITS 6
+#define CAN_MESSAGE_FILTER "612"
+#define CAN_MESSAGE_MASK "0E0"
 #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
@@ -30,7 +31,7 @@
 #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_ID_AND_DATA_SIZE "6128"
 #define CAN_MESSAGE_FUEL_LEVEL_INDEX 10
 #define CAN_MESSAGE_FUEL_LEVEL_FORMAT 2
 //Wheel ticks frame: 50D700091508EC8E90
diff --git a/src/vehicle-gateway/common.h b/src/vehicle-gateway/common.h
index cfc8041..a414cca 100644
--- a/src/vehicle-gateway/common.h
+++ b/src/vehicle-gateway/common.h
@@ -20,6 +20,7 @@
 #include <inttypes.h>
 
 #define CR '\r'
+#define LF '\n'
 #define CR_LF "\r\n"
 #define EOS '\0'
 #define SPACE ' '
diff --git a/src/vehicle-gateway/obd2.cpp b/src/vehicle-gateway/obd2.cpp
index d9dfe96..de269b9 100644
--- a/src/vehicle-gateway/obd2.cpp
+++ b/src/vehicle-gateway/obd2.cpp
@@ -71,14 +71,20 @@ included by <termios.h> */
 #define ELM_GET_ID "AT I\r\n"
 #define ELM_PROMPT '>'
 #define ELM_READ_LOOP 5000 //5 ms
-#define ELM_READ_TIMEOUT 1000000 //100 ms
+#define ELM_READ_TIMEOUT 100000 //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_PROTOCOL "AT SP"
+#define ELM_SET_PROTOCOL_LENGTH 10
 #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 ELM_LINEFEEDS_ON "AT L1\r\n"
+#define ELM_PRINTING_SPACES_OFF "AT S0\r\n"
+#define ELM_ALLOW_LONG_MESSAGES "AT AL\r\n"
+#define ELM_DISPLAY_DLC_ON "AT D1\r\n"
 
 #define BUFFER_MAX_LENGTH 512
 
@@ -216,6 +222,23 @@ bool obd2_send_command(const char* cmd)
         return false;
 }
 
+bool obd2_command(const char* cmd, uint64_t& timestamp)
+{
+    char* answer;
+    size_t answer_length;
+    if (obd2_send_command(cmd)){
+        answer=NULL;
+        if(obd2_read(answer,answer_length,timestamp)!=true){
+            return false;
+        }else{
+            delete answer;
+        }
+    }else{
+        return false;
+    }
+    return true;
+}
+
 bool obd2_init(char* obd2_device, unsigned int baudrate)
 {
     bool retval = false;
@@ -345,73 +368,38 @@ bool obd2_read_fuel_level(uint8_t& level,uint64_t& timestamp)
     return true;
 }
 
-bool obd2_set_filter(uint16_t filter,uint16_t mask,uint64_t& timestamp)
+bool obd2_config_can_reader(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];
+    char protocolBuffer[ELM_SET_PROTOCOL_LENGTH];
 
-    sprintf(filterBuffer,ELM_SET_CAN_ID_FILTER " %x" CR_LF,filter);
-    sprintf(maskBuffer,ELM_SET_CAN_ID_MASK " %x" CR_LF,mask);
+    sprintf(filterBuffer,ELM_SET_CAN_ID_FILTER " %s" CR_LF,CAN_MESSAGE_FILTER);
+    sprintf(maskBuffer,ELM_SET_CAN_ID_MASK " %s" CR_LF,CAN_MESSAGE_MASK);
+    sprintf(protocolBuffer,ELM_SET_PROTOCOL " %d" CR_LF,CAN_PROTOCOL_11_BIT_ID_500KBITS);
 
-    if (obd2_send_command(filterBuffer)){
-        answer=NULL;
-        if(obd2_read(answer,answer_length,timestamp)!=true){
-            return false;
-        }else{
-            delete answer;
-        }
-    }else{
+    if(!obd2_command(ELM_GET_ID,timestamp))
         return false;
-    }
-    if (obd2_send_command(maskBuffer)){
-        answer=NULL;
-        if(obd2_read(answer,answer_length,timestamp)!=true){
-            return false;
-        }else{
-            delete answer;
-        }
-    }else{
+    if(!obd2_command(ELM_LINEFEEDS_ON,timestamp))
         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{
+    if(!obd2_command(ELM_HEADER_ON,timestamp))
         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{
+    if(!obd2_command(ELM_PRINTING_SPACES_OFF,timestamp))
         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{
+    if(!obd2_command(ELM_ALLOW_LONG_MESSAGES,timestamp))
+        return false;
+    if(!obd2_command(protocolBuffer,timestamp))
+        return false;
+    if(!obd2_command(ELM_DISPLAY_DLC_ON,timestamp))
+        return false;
+    if(!obd2_command(ELM_CAN_FORMAT_OFF,timestamp))
+        return false;
+    if(!obd2_command(maskBuffer,timestamp))
+        return false;
+    if(!obd2_command(filterBuffer,timestamp))
+        return false;
+    if(!obd2_send_command(ELM_MONITOR_ALL))
         return false;
-    }
     return true;
 }
 
@@ -440,7 +428,7 @@ can_message_id_t can_read(char*& data,uint64_t& timestamp)
                     delete tmp; //free the buffer
                     break;
                 }else{
-                    if(buf==CR){
+                    if(buf==LF){
                         isRead=true;
                         *(tmp+buf_length)=EOS;
                         timestamp=get_timestamp();
diff --git a/src/vehicle-gateway/obd2.h b/src/vehicle-gateway/obd2.h
index 871e68a..9097943 100644
--- a/src/vehicle-gateway/obd2.h
+++ b/src/vehicle-gateway/obd2.h
@@ -56,8 +56,6 @@ 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
diff --git a/src/vehicle-gateway/veh-gateway.cpp b/src/vehicle-gateway/veh-gateway.cpp
index 5956495..359a343 100644
--- a/src/vehicle-gateway/veh-gateway.cpp
+++ b/src/vehicle-gateway/veh-gateway.cpp
@@ -305,6 +305,8 @@ int main(int argc, char* argv[])
     if(!obd2_reset(stop)){
         LOG_DEBUG(gContext,"RESET OBD2 FAILURE [DURATION = %" PRIu64 " ms]", stop-start);
         return(-1);
+    }else{
+        LOG_DEBUG(gContext,"RESET OBD2 OK [DURATION = %" PRIu64 " ms]", stop-start);
     }
 
     if(can_reader_mode){
@@ -314,11 +316,6 @@ int main(int argc, char* argv[])
             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,"SET FILTER OBD2 FAILURE [DURATION = %" PRIu64 " ms]", stop-start);
-            return(-1);
-        }
     }else{
         // config standard OBD2 with AT command
         start = get_timestamp();