From 799b0a073bfa95ff351a80ae08b74adbd150d793 Mon Sep 17 00:00:00 2001 From: asanoaozora Date: Tue, 7 Nov 2017 15:25:41 +0100 Subject: extend vehicle gateway --- src/vehicle-gateway/can.h | 53 +++++++++ src/vehicle-gateway/common.h | 5 + src/vehicle-gateway/obd2.cpp | 209 ++++++++++++++++++++++++------------ src/vehicle-gateway/obd2.h | 9 +- src/vehicle-gateway/veh-gateway.cpp | 187 ++++++++++++++++++++++---------- test/script/dashboard.bmp | Bin 0 -> 800138 bytes 6 files changed, 334 insertions(+), 129 deletions(-) create mode 100644 src/vehicle-gateway/can.h create mode 100644 test/script/dashboard.bmp 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 +* +* \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 +#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 #include +#include /* baudrate settings are defined in , which is included by */ @@ -53,10 +54,10 @@ included by */ #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 */ #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)&&(timeout0) + { + 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 #include #include +#include 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 ×tamp); -bool can_read_engine_rpm(uint16_t& rpm, uint64_t ×tamp); - -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 ×tamp); + #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 #include #include +#include #include #include #include +#include #include #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 Binary files /dev/null and b/test/script/dashboard.bmp differ -- cgit v1.2.1