add DLT to FSA, align with latest AMB

8 files changed, 93 insertions, 35 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c8d3643..b2bec7d 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -17,6 +17,7 @@
 ###########################################################################
 project(fuel-stop-advisor)
 cmake_minimum_required(VERSION 2.8)
+find_package(PkgConfig)
 
 option(WITH_DEBUG
         "Enable the debug messages" OFF)
@@ -61,7 +62,7 @@ if(NOT EXISTS ${AUTOMOTIVE_MESSAGE_BROKER_DIR})
         ExternalProject_Add(${AUTOMOTIVE_MESSAGE_BROKER}
         PREFIX ${AUTOMOTIVE_MESSAGE_BROKER_DIR}
         GIT_REPOSITORY  https://github.com/otcshare/automotive-message-broker.git
-        GIT_TAG c216955d16ca275159891cad296217094d972390
+        GIT_TAG 8af1cc9a502cb85736b7afb37dc9073a168aa605
         UPDATE_COMMAND ""
         PATCH_COMMAND ""
         INSTALL_COMMAND sudo make install
@@ -79,6 +80,13 @@ if(NOT EXISTS ${AUTOMOTIVE_MESSAGE_BROKER_DIR})
     endif()
 endif()
 
+if(${WITH_DLT})
+    add_definitions("-DDLT_ENABLED=1")
+    pkg_check_modules(DLT REQUIRED automotive-dlt)
+    include_directories( ${DLT_INCLUDE_DIRS} )
+    set(LIBRARIES ${LIBRARIES} ${DLT_LIBRARIES})
+endif()
+
 add_subdirectory(navigation/src/navigation "${CMAKE_CURRENT_BINARY_DIR}/navigation")
 set(DBUS_GENERATED_INCLUDE_DIR "${CMAKE_CURRENT_BINARY_DIR}/navigation/dbus-include")
 
diff --git a/src/fuel-stop-advisor/CMakeLists.txt b/src/fuel-stop-advisor/CMakeLists.txt
index 5f78033..e99c0fd 100644
--- a/src/fuel-stop-advisor/CMakeLists.txt
+++ b/src/fuel-stop-advisor/CMakeLists.txt
@@ -70,6 +70,7 @@ endforeach()
 add_executable(fuel-stop-advisor fuel-stop-advisor.cpp  ${TRIPCOMPUTER_DIR}/ctripcomputer.cpp)
 
 set(LIBRARIES
+    ${LIBRARIES}
     ${DBUS_CPP_LIBRARIES}
     ${GLIB_LIBRARIES}
     ${DBUS_CPP_GLIB_LIBRARIES}
diff --git a/src/fuel-stop-advisor/fuel-stop-advisor.cpp b/src/fuel-stop-advisor/fuel-stop-advisor.cpp
index 4177028..071fbec 100644
--- a/src/fuel-stop-advisor/fuel-stop-advisor.cpp
+++ b/src/fuel-stop-advisor/fuel-stop-advisor.cpp
@@ -12,13 +12,16 @@
 #include "genivi-navigationcore-constants.h"
 #include "genivi-navigationcore-routing_proxy.h"
 
+#include "log.h"
+
+DLT_DECLARE_CONTEXT(gCtx);
+
+
 static DBus::Glib::BusDispatcher dispatcher;
 static DBus::Connection *conn;
 static class FuelStopAdvisor *server;
 static GMainLoop *loop;
 
-#define dbgprintf(...) printf(__VA_ARGS__);
-
 #if (!DEBUG_ENABLED)
 #undef dbgprintf
 #define dbgprintf(...) ;
@@ -314,7 +317,7 @@ class FuelStopAdvisor
 	double enhancedDistance(double level, double &remaining)
 	{
 		double distance=0;
-        dbgprintf("routeHandle %d\n",routeHandle);
+        LOG_INFO(gCtx,"routeHandle %d\n",routeHandle);
         if (routeHandle) {
             std::vector< std::map< int32_t, ::DBus::Struct< uint8_t, ::DBus::Variant > > > RouteShape;
             std::vector< int32_t > valuesToReturn;
@@ -343,10 +346,10 @@ class FuelStopAdvisor
 					level-=fuel_consumption;
 				}
 			}
-            dbgprintf("%d segments\n",totalNumberOfSegments);
+            LOG_INFO(gCtx,"%d segments\n",totalNumberOfSegments);
 		}
 		remaining=level/fuel_consumption_l_100km*100;
-        dbgprintf("distance_on_route %f remaining %f\n",distance/1000,remaining);
+        LOG_INFO(gCtx,"distance_on_route %f remaining %f\n",distance/1000,remaining);
 		return distance/1000+(remaining > 0 ? remaining:0);
 	}
 	
@@ -433,9 +436,9 @@ class FuelStopAdvisor
         double remaining;
         if (advisorMode) {
             enhancedDistance(fuelLevel, remaining);
-            dbgprintf("Advisor %f vs %d\n",remaining, distanceThreshold);
+            LOG_INFO(gCtx,"Advisor %f vs %d\n",remaining, distanceThreshold);
             if (remaining < distanceThreshold) {
-                dbgprintf("Warning %f < %d\n",remaining, distanceThreshold);
+                LOG_INFO(gCtx,"Warning %f < %d\n",remaining, distanceThreshold);
                 destinationCantBeReached = true;
             }
             else
@@ -450,7 +453,6 @@ class FuelStopAdvisor
 	void
 	SetFuelAdvisorSettings(const bool& advisorMode, const uint8_t& distanceThreshold)
 	{
-        dbgprintf("SetFuelAdvisorSettings(%d,%d)\n",advisorMode, distanceThreshold);
 		this->advisorMode=advisorMode;
 		this->distanceThreshold=distanceThreshold;
         updateEnhancedDistance();
@@ -466,14 +468,12 @@ class FuelStopAdvisor
 
 	void SetRouteHandle(const uint32_t& routeHandle)
 	{
-        dbgprintf("SetRouteHandle %d\n",routeHandle);
         this->routeHandle=routeHandle;
         updateEnhancedDistance();
 	}
 
     void ReleaseRouteHandle(const uint32_t& routeHandle)
     {
-        dbgprintf("ResetRouteHandle %d\n",routeHandle);
         this->routeHandle=0;
         updateEnhancedDistance();
     }
@@ -520,7 +520,10 @@ update_enhanced_distance(gpointer user_data)
 
 int main(int argc, char **argv)
 {
-	loop=g_main_loop_new(NULL, false);
+    DLT_REGISTER_APP("FSAD","FUEL STOP ADVISOR");
+    DLT_REGISTER_CONTEXT(gCtx,"FSAD","Global Context");
+
+    loop=g_main_loop_new(NULL, false);
 	dispatcher.attach(NULL);
 	DBus::default_dispatcher = &dispatcher;
 	conn = new DBus::Connection(DBus::Connection::SessionBus());
diff --git a/src/update_third_party.sh b/src/update_third_party.sh
index 8b07be1..99bbd58 100755
--- a/src/update_third_party.sh
+++ b/src/update_third_party.sh
@@ -2,7 +2,7 @@
 
 navigation=0
 navit=0
-navigation_version='56837f0237fadedd325940630601061bd7bbd30f'
+navigation_version='4c3e24b04f8ff1e41a94f1c1dd181ae3412c3db9'
 positioning_version='9725fe1f553197042d6445997690d452a73490c0'
 navit_version='1e71b5fd4c0bf5ac96e5207c51db7d17057ed798'
 
diff --git a/src/vehicle-gateway/CMakeLists.txt b/src/vehicle-gateway/CMakeLists.txt
index 4157899..9eb6721 100644
--- a/src/vehicle-gateway/CMakeLists.txt
+++ b/src/vehicle-gateway/CMakeLists.txt
@@ -24,6 +24,14 @@ cmake_minimum_required(VERSION 2.8)
 
 message(STATUS ${PROJECT_NAME})
 
+option(WITH_DEBUG
+        "Enable the debug messages" OFF)
+option(WITH_DLT
+        "Enable DLT logging" OFF)
+
+message(STATUS "WITH_DEBUG = ${WITH_DEBUG}")
+message(STATUS "WITH_DLT = ${WITH_DLT}")
+
 add_definitions("-std=gnu++11")
 
 if(WITH_DEBUG)
diff --git a/src/vehicle-gateway/obd2.cpp b/src/vehicle-gateway/obd2.cpp
index 1338284..a32e1ce 100644
--- a/src/vehicle-gateway/obd2.cpp
+++ b/src/vehicle-gateway/obd2.cpp
@@ -49,6 +49,8 @@
 /* baudrate settings are defined in <asm/termbits.h>, which is
 included by <termios.h> */
 
+#define OBD_GET_PID_LIST "0100\r\n"
+
 #define OBD_HEADER_LENGTH 5 //41 0C for instance
 
 #define OBD_FUEL_TANK_PID "012F\r\n"
@@ -64,6 +66,7 @@ included by <termios.h> */
 #define OBD_VEH_SPEED_PID "010D\r\n"
 
 #define ELM_RESET_ALL "AT Z\r\n"
+#define ELM_ECHO_OFF "AT E0\r\n"
 #define ELM_GET_ID "AT I\r\n"
 #define ELM_PROMPT '>'
 #define ELM_READ_LOOP 5000 //5 ms
@@ -217,11 +220,10 @@ bool obd2_init(char* obd2_device, unsigned int baudrate)
     return retval;
 }
 
-bool obd2_reset()
+bool obd2_reset(uint64_t& timestamp)
 {
     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){
@@ -233,6 +235,29 @@ bool obd2_reset()
     return true;
 }
 
+bool obd2_config(uint64_t& timestamp)
+{
+    char* answer;
+    size_t answer_length;
+    if (obd2_send_command(ELM_ECHO_OFF)){
+        answer=NULL;
+        if(obd2_read_answer(answer,answer_length,timestamp)!=true){
+            return false;
+        }
+    }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_read_engine_rpm(uint16_t& rpm,uint64_t& timestamp)
 {
     //`010C` Engine RPM: returns 2 bytes (A,B): RPM [1/min] = ((A*256)+B)/4
diff --git a/src/vehicle-gateway/obd2.h b/src/vehicle-gateway/obd2.h
index 278e29a..98fa69c 100644
--- a/src/vehicle-gateway/obd2.h
+++ b/src/vehicle-gateway/obd2.h
@@ -45,7 +45,9 @@ extern "C" {
 
 bool obd2_init(char* obd2_device, unsigned int baudrate);
 
-bool obd2_reset();
+bool obd2_reset(uint64_t &timestamp);
+
+bool obd2_config(uint64_t& timestamp);
 
 bool obd2_read_engine_rpm(uint16_t& rpm, uint64_t &timestamp);
 
diff --git a/src/vehicle-gateway/veh-gateway.cpp b/src/vehicle-gateway/veh-gateway.cpp
index 3e0ac33..a835936 100644
--- a/src/vehicle-gateway/veh-gateway.cpp
+++ b/src/vehicle-gateway/veh-gateway.cpp
@@ -85,6 +85,7 @@ bool get_geolocation(char*& sock_buf,char* buffer,const uint64_t timestamp)
     char *token;
     uint8_t cnt=0;
     bool retval = false;
+    double fract;
     geolocation.latitude=0;
     geolocation.longitude=0;
     geolocation.altitude=0;
@@ -98,18 +99,20 @@ bool get_geolocation(char*& sock_buf,char* buffer,const uint64_t timestamp)
             if(strcmp(token,NMEA_DATA_VALID)==0){
                 retval=true;
             }else{
-                LOG_INFO_MSG(gContext,"NMEA DATA NOT VALID\n");
+                LOG_INFO_MSG(gContext,"NMEA DATA NOT VALID");
                 return retval;
             }
             break;
         case NMEA_RMC_LATITUDE:
-            geolocation.latitude=atof(token);
+            fract=modf(atof(token)/100,&(geolocation.latitude));
+            geolocation.latitude+=(fract*100)/60;
             break;
         case NMEA_RMC_LATITUDE_INDICATOR:
             if(token==NMEA_SOUTH) geolocation.latitude=(-1)*geolocation.latitude;
             break;
         case NMEA_RMC_LONGITUDE:
-            geolocation.longitude=atof(token);
+            fract=modf(atof(token)/100,&(geolocation.longitude));
+            geolocation.longitude+=(fract*100)/60;
             break;
         case NMEA_RMC_LONGITUDE_INDICATOR:
             if(token==NMEA_WEST) geolocation.longitude=(-1)*geolocation.longitude;
@@ -122,7 +125,7 @@ bool get_geolocation(char*& sock_buf,char* buffer,const uint64_t timestamp)
         }
         cnt++;
     }
-    LOG_DEBUG(gContext,"Lat: %f Lon: %f Alt: %f\n",geolocation.latitude,geolocation.longitude,geolocation.altitude);
+    LOG_DEBUG(gContext,"Lat: %f Lon: %f Alt: %f",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",timestamp,"0$GVGNSP",timestamp,geolocation.latitude,geolocation.longitude,geolocation.altitude);
@@ -136,10 +139,10 @@ bool get_engine_speed(char*& sock_buf)
     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");
+        LOG_ERROR_MSG(gContext,"Read engine rpm failed");
         return false;
     }else{
-        LOG_DEBUG(gContext,"Engine speed: %d\n",rpm);
+        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);
@@ -154,10 +157,10 @@ bool get_fuel_tank_level(char*& sock_buf)
     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");
+        LOG_ERROR_MSG(gContext,"Read fuel tank level failed");
         return false;
     }else{
-        LOG_DEBUG(gContext,"Fuel tank level: %d\%\n",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);
@@ -249,12 +252,12 @@ int main(int argc, char* argv[])
     stop = get_timestamp();
     if (result)
     {
-      LOG_INFO(gContext,"INIT OBD2 OK [DURATION = %" PRIu64 " ms]\n", stop-start);
+      LOG_INFO(gContext,"INIT OBD2 OK [DURATION = %" PRIu64 " ms]", stop-start);
     }
     else
     {
-      LOG_DEBUG(gContext,"INIT OBD2 FAILURE [DURATION = %" PRIu64 " ms]\n", stop-start);
-      LOG_DEBUG(gContext,"Do you have access rights to %s ?\n", modem_device_obd2);
+      LOG_DEBUG(gContext,"INIT OBD2 FAILURE [DURATION = %" PRIu64 " ms]", stop-start);
+      LOG_DEBUG(gContext,"Do you have access rights to %s ?", modem_device_obd2);
       return(-1);
     }
 
@@ -264,18 +267,26 @@ int main(int argc, char* argv[])
     stop = get_timestamp();
     if (result)
     {
-      LOG_INFO(gContext,"INIT GNSS OK [DURATION = %" PRIu64 " ms]\n", stop-start);
+      LOG_INFO(gContext,"INIT GNSS OK [DURATION = %" PRIu64 " ms]", stop-start);
     }
     else
     {
-      LOG_DEBUG(gContext,"INIT GNSS FAILURE [DURATION = %" PRIu64 " ms]\n", stop-start);
-      LOG_DEBUG(gContext,"Do you have access rights to %s ?\n", modem_device_gnss);
+      LOG_DEBUG(gContext,"INIT GNSS FAILURE [DURATION = %" PRIu64 " ms]", stop-start);
+      LOG_DEBUG(gContext,"Do you have access rights to %s ?", modem_device_gnss);
       return(-1);
     }
 
     // reset the OBD2 device
-    if(!obd2_reset()){
-        LOG_ERROR_MSG(gContext,"RESET OBD2 FAILURE\n");
+    start = get_timestamp();
+    if(!obd2_reset(stop)){
+        LOG_DEBUG(gContext,"RESET OBD2 FAILURE [DURATION = %" PRIu64 " ms]", stop-start);
+        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);
     }
 
@@ -296,9 +307,9 @@ 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\n",ipaddr,PORT1);
-                LOG_DEBUG(gContext,"Len:%d\n", (int)strlen(sock_buf));
-                LOG_DEBUG(gContext,"Data:%s\n", sock_buf);
+                LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT1);
+                LOG_DEBUG(gContext,"Len:%d", (int)strlen(sock_buf));
+                LOG_DEBUG(gContext,"Data:%s", sock_buf);
 
                 si_other.sin_port = htons(PORT1);
                 if(sendto(sock, sock_buf, strlen(sock_buf)+1, 0, (struct sockaddr *)&si_other, slen) == -1)