start integrating vehicle gateway

14 files changed, 2206 insertions, 13 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f098236..c8d3643 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -28,12 +28,18 @@ option(WITH_HTML_MIGRATION
         "Enable migration to the html based hmi" OFF)
 option(WITH_STYLESHEET
         "Generate the style sheet" OFF)
+option(WITH_VEHICLE_GATEWAY
+        "Use of vehicle gateway" OFF)
+option(WITH_DLT
+        "Enable DLT logging" OFF)
 
 message(STATUS "WITH_STYLESHEET = ${WITH_STYLESHEET}")
 message(STATUS "WITH_DEBUG = ${WITH_DEBUG}")
 message(STATUS "WITH_PLUGIN_MIGRATION = ${WITH_PLUGIN_MIGRATION}")
 message(STATUS "AMB_ON_DBUS_SESSION = ${AMB_ON_DBUS_SESSION}")
 message(STATUS "WITH_HTML_MIGRATION = ${WITH_HTML_MIGRATION}")
+message(STATUS "WITH_VEHICLE_GATEWAY = ${WITH_VEHICLE_GATEWAY}")
+message(STATUS "WITH_DLT = ${WITH_DLT}")
 
 set(NAVIGATION_API_DIR "${CMAKE_CURRENT_SOURCE_DIR}/navigation/api")
 set(POSITIONING_API_DIR "${CMAKE_CURRENT_SOURCE_DIR}/navigation/src/navigation/positioning/enhanced-position-service/dbus/api")
@@ -91,3 +97,7 @@ add_subdirectory(navigation/src/poi-service "${CMAKE_CURRENT_BINARY_DIR}/poi-ser
 if(WITH_HTML_MIGRATION)
     add_subdirectory(hmi/html)
 endif()
+
+if(WITH_VEHICLE_GATEWAY)
+    add_subdirectory(vehicle-gateway)
+endif()
diff --git a/src/build.sh b/src/build.sh
index 9000332..4c747cd 100755
--- a/src/build.sh
+++ b/src/build.sh
@@ -5,7 +5,8 @@ html="OFF"
 clean=0
 capi=0
 navit=0
-theme_option="-DWITH_STYLESHEET=OFF"
+gateway="OFF"
+theme_option="OFF"
 pack_for_gdp=0
 commonapi_tools_option=""
 
@@ -35,7 +36,7 @@ function check_path_for_capi
 	commonapi_tools_option="-DDBUS_LIB_PATH="$DBUS_LIB_PATH" -DCOMMONAPI_DBUS_TOOL_DIR="$COMMONAPI_DBUS_TOOL_DIR" -DCOMMONAPI_TOOL_DIR="$COMMONAPI_TOOL_DIR""
 }
 
-while getopts cdmhnt opt
+while getopts cdmghnt opt
 do
 	case $opt in
 	c)
@@ -47,6 +48,9 @@ do
 	m)
 		capi=1
 		;;
+	g)
+		gateway="ON"
+		;;
 	h)
 		html="ON"
 		;;
@@ -54,7 +58,7 @@ do
 		navit=1
 		;;
 	t)
-		theme_option="-DWITH_STYLESHEET=ON"
+		theme_option="ON"
 		pack_for_gdp=1
 		;;
 	\?)
@@ -62,8 +66,9 @@ do
 		echo "$0 [-cdmhnt]"
 		echo "-c: Rebuild with clean"
 		echo "-d: Enable the debug messages"
-		echo "-m: Build with commonAPI plugins "
 		echo "-h: Enable migration to the html based hmi"
+		echo "-g: Build the vehicle gateway"
+		echo "-m: Build with commonAPI plugins "
 		echo "-n: Build navit"
 		echo "-t: Generate the HMI theme"
 		exit 1
@@ -126,9 +131,9 @@ if [ "$clean" = 1 ]
 then
 	if [ "$capi" = 0 ]
 	then
-		cmake $theme_option -DWITH_HTML_MIGRATION=$html -DWITH_PLUGIN_MIGRATION=OFF -DWITH_DEBUG=$debug ../
+		cmake -DWITH_STYLESHEET=$theme_option -DWITH_VEHICLE_GATEWAY=$gateway -DWITH_HTML_MIGRATION=$html -DWITH_PLUGIN_MIGRATION=OFF -DWITH_DEBUG=$debug ../
 	else
-		cmake $theme_option -DWITH_HTML_MIGRATION=$html -DWITH_PLUGIN_MIGRATION=ON -DWITH_DBUS_INTERFACE=OFF $commonapi_tools_option -DWITH_DEBUG=$debug ../
+		cmake -DWITH_STYLESHEET=$theme_option -DWITH_VEHICLE_GATEWAY=$gateway -DWITH_HTML_MIGRATION=$html -DWITH_PLUGIN_MIGRATION=ON -DWITH_DBUS_INTERFACE=OFF $commonapi_tools_option -DWITH_DEBUG=$debug ../
 		echo 'fix a bug in the generation of CommonAPI hpp'
 		sed -i -e 's/(const TimeStampedEnum::/(const ::v4::org::genivi::navigation::navigationcore::NavigationCoreTypes::TimeStampedEnum::/' ./navigation/franca/src-gen/v4/org/genivi/navigation/navigationcore/LocationInput.hpp
 		sed -i -e 's/(const TimeStampedEnum::/(const ::v4::org::genivi::navigation::navigationcore::NavigationCoreTypes::TimeStampedEnum::/' ./navigation/franca/src-gen/v4/org/genivi/navigation/navigationcore/MapMatchedPosition.hpp
diff --git a/src/clone_and_build.sh b/src/clone_and_build.sh
index 6b1f306..ee6d61b 100755
--- a/src/clone_and_build.sh
+++ b/src/clone_and_build.sh
@@ -1,9 +1,9 @@
 #!/bin/bash
 
 build_option=""
-navigation_version='312fedf2360633e7d76f9a70a17c5d4961964e34'
+navigation_version='c72289b6a9b82723f38d991629de8677a260f9fd'
 positioning_version='9725fe1f553197042d6445997690d452a73490c0'
-navit_version='5ca60166243b0671b4a7e62938b13feacd2391e2'
+navit_version='1e71b5fd4c0bf5ac96e5207c51db7d17057ed798'
 
 echo "version of navigation is: $navigation_version"
 echo "version of positioning is: $positioning_version"
diff --git a/src/kill-all b/src/kill-all
index 6d87216..4f440f0 100755
--- a/src/kill-all
+++ b/src/kill-all
@@ -1,4 +1,4 @@
 #!/bin/sh
 echo 'kill all remaining process'
-kill -9 `ps -ef | grep '\(navit\|poi-server\|hmi-launcher\|fuel-stop-advisor\|ambd\|enhanced-position-service\)' | grep -v grep | awk '{print $2}'`
+kill -9 `ps -ef | grep '\(navit\|poi-server\|hmi-launcher\|fuel-stop-advisor\|ambd\|vehicle-gateway\|enhanced-position-service\)' | grep -v grep | awk '{print $2}'`
 
diff --git a/src/run b/src/run
index 9ab20d2..3af9f22 100755
--- a/src/run
+++ b/src/run
@@ -102,6 +102,7 @@ xterm=0 #no subprocess into a separate xterm
 log=0 #no log file 
 enhpos=1 #enhanced position server enabled 
 replayer=1 #replayer enabled
+gateway=0 #vehicle gateway disabled
 lm=0 #layer manager disabled
 wm="" #to store the current window manager (in case of start with the layer manager)
 verbose=0 #no debug or log messages displayed
@@ -156,9 +157,13 @@ HMI_LAUNCHER=hmi-launcher
 HMI_LAUNCHER_SRC_DIR=$SRC_DIR/hmi/$HMI_LAUNCHER
 HMI_LAUNCHER_BIN_DIR=$BIN_DIR/hmi/$HMI_LAUNCHER
 
+VEHICLE_GATEWAY=vehicle-gateway
+VEHICLE_GATEWAY_SRC_DIR=$SRC_DIR/$VEHICLE_GATEWAY
+VEHICLE_GATEWAY_BIN_DIR=$BIN_DIR/$VEHICLE_GATEWAY
+devices="/dev/ttyUSB0 /dev/ttyACM0"
 
 # options analysis
-while getopts a:c:f:glnorvx opt
+while getopts a:c:df:g:lnorvx opt
 do
 	case $opt in
 	a) #select another hmi panel
@@ -184,11 +189,15 @@ do
 			;;
 		esac
 		;;
+	d) #enable the debugger
+		gdb=1
+		;;
 	f) #load another poi database
 		poidatabase=$(readlink -f $OPTARG) 
 		;;
-	g) #enable the debugger
-		gdb=1
+	g) #enable the vehicle gateway
+		gateway=1
+		devices=$OPTARG
 		;;
 	l) #enable the layer manager
 		lm=1
@@ -213,7 +222,8 @@ do
 		echo "$0 [-a application][-c center][-glnorvx]"
 		echo "-a: Set application (default application.qml)"
 		echo "-c: Set center (supported values: paris,tokyo,longitude latitude). Default is geneve"
-		echo "-g: Run subprocesses within gdb (only with -x)"
+		echo "-d: Run subprocesses within gdb (only with -x)"
+		echo "-g: Run the vehicle gateway (only with -r) -g \"<ELM327device> <GNSSdevice>\""
 		echo "-l: Use layermanager"
 		echo "-n: Don't start enhanced positioning service"
 		echo "-o: Create log file of subprocess output"
@@ -301,6 +311,11 @@ wait_for_service org.genivi.navigationcore.Session /org/genivi/navigationcore
 if [ "$replayer" = 1 ]
 then # start the log replayer (of the fsa) with a sample log file
 	run "Log Replayer" $LOG_REPLAYER_BIN_DIR/$LOG_REPLAYER  $LOG_REPLAYER_SRC_DIR/logs/geneve-cologny.log
+else
+	if [ "$gateway" = 1 ]
+	then
+		run "Vehicle gateway" $VEHICLE_GATEWAY_BIN_DIR/$VEHICLE_GATEWAY $devices
+	fi
 fi
 
 sleep 3 # need a sleep here (to be improved)
diff --git a/src/vehicle-gateway/CMakeLists.txt b/src/vehicle-gateway/CMakeLists.txt
new file mode 100644
index 0000000..cb3824e
--- /dev/null
+++ b/src/vehicle-gateway/CMakeLists.txt
@@ -0,0 +1,61 @@
+###########################################################################
+# @licence app begin@
+# SPDX-License-Identifier: MPL-2.0
+#
+# Component Name: Vehicle gateway
+#
+# Author: Philippe Colliot
+#
+# Copyright (C) 2017, PSA GROUP
+#
+# Former code made by Marco Residori
+#
+# Copyright (C) 2013, XS Embedded GmbH
+# 
+# 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@
+###########################################################################
+project(vehicle-gateway)
+cmake_minimum_required(VERSION 2.8)
+
+message(STATUS ${PROJECT_NAME})
+
+add_definitions("-std=gnu++11")
+
+message(STATUS "WITH_DLT = ${WITH_DLT}")
+
+find_package(PkgConfig REQUIRED)
+
+set(PRJ_SRCS
+    ${CMAKE_CURRENT_SOURCE_DIR}/veh-gateway.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/obd2.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/hnmea.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/gnss.cpp
+)
+
+include_directories(
+    ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+set(LIBRARIES pthread)
+
+if(WITH_DLT)
+    pkg_check_modules(DLT REQUIRED automotive-dlt)
+    add_definitions("-DDLT_ENABLED=1")
+    include_directories( ${DLT_INCLUDE_DIRS} )
+    set(LIBRARIES ${LIBRARIES} ${DLT_LIBRARIES})
+endif()
+
+add_executable(${PROJECT_NAME} ${PRJ_SRCS})
+
+target_link_libraries(${PROJECT_NAME} ${LIBRARIES})
+
+install(TARGETS ${PROJECT_NAME} DESTINATION bin)
+
+
+
+
diff --git a/src/vehicle-gateway/gnss.cpp b/src/vehicle-gateway/gnss.cpp
new file mode 100644
index 0000000..faa4e10
--- /dev/null
+++ b/src/vehicle-gateway/gnss.cpp
@@ -0,0 +1,267 @@
+/**
+* @licence app begin@
+* SPDX-License-Identifier: MPL-2.0
+*
+* \copyright Copyright (C) 2017, PSA GROUP
+*
+* \file obd2.c
+*
+* \brief This file is part of the FSA project.
+*
+* \author Philippe Colliot <philippe.colliot@mpsa.com>
+*
+* \version 0.1
+*
+*
+* @ref http://tldp.org/HOWTO/Serial-Programming-HOWTO/x115.html
+*
+* Part of this code has been inspired by Helmut Schmidt <https://github.com/huirad>
+* and comes from the positioning project <https://github.com/GENIVI/positioning>
+*
+*
+* This Source Code Form is subject to the terms of the
+* Mozilla Public License (MPL), 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/.
+*
+* For further information see http://www.genivi.org/.
+*
+* List of changes:
+* <date>, <name>, <description of change>
+*
+* @licence end@
+*/
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <string.h>
+#include <time.h>
+#include <inttypes.h>
+
+//#include <sys/timeb.h>
+
+#include <gnss.h>
+#include <hnmea.h>
+
+const char* ACTIVATE_GST = "$PUBX,40,GST,0,0,0,1,0,0*5A\r\n";
+const char* ACTIVATE_GRS = "$PUBX,40,GRS,0,0,0,1,0,0*5C\r\n";
+
+static int g_gnss_fd = -1;
+static struct termios g_oldtio;
+static char* g_gnss_device;
+static unsigned int g_baudrate;
+
+pthread_t g_thread_gnss;
+pthread_mutex_t mutex_gnss;
+char gnssBuffer[MAX_GNSS_BUFFER_SIZE];
+bool gnssDataReady;
+
+/** Flag to terminate NMEA reader thread */
+volatile int g_gnss_loop=1;
+
+/** Maximum number of retries to re-open GNSS_DEVICE when select() returns an error */
+#define OPEN_RETRY_MAX 15
+/** Delay between retries in seconds */
+#define OPEN_RETRY_DELAY 2
+
+int gnss_open_device(char* gnss_device, unsigned int baudrate)
+{
+  int fd;
+  struct termios newtio;
+
+    /*
+      Open modem device for reading and writing and not as controlling tty
+      because we don't want to get killed if linenoise sends CTRL-C.
+    */
+    fd = open(gnss_device, O_RDWR | O_NOCTTY );
+    if (fd <0) {perror(gnss_device); return (-1); }
+
+    tcgetattr(fd,&g_oldtio); /* save current serial port settings */
+    bzero(&newtio, sizeof(newtio)); /* clear struct for new port settings */
+
+    /*
+      BAUDRATE: Set bps rate. You could also use cfsetispeed and cfsetospeed.
+      CRTSCTS : output hardware flow control (only used if the cable has
+                all necessary lines. See sect. 7 of Serial-HOWTO)
+      CS8     : 8n1 (8bit,no parity,1 stopbit)
+      CLOCAL  : local connection, no modem contol
+      CREAD   : enable receiving characters
+    */
+    newtio.c_cflag = baudrate | CS8 | CLOCAL | CREAD;
+ 
+    /*
+      IGNPAR  : ignore bytes with parity errors
+      ICRNL   : map CR to NL (otherwise a CR input on the other computer
+                will not terminate input)
+      otherwise make device raw (no other input processing)
+    */
+    newtio.c_iflag = IGNPAR;
+ 
+    /*
+     Raw output.
+    */
+    newtio.c_oflag = 0;
+ 
+    /*
+      ICANON  : enable canonical input
+      disable all echo functionality, and don't send signals to calling program
+    */
+    newtio.c_lflag = ICANON;
+ 
+    /*
+      initialize all control characters
+      default values can be found in /usr/include/termios.h, and are given
+      in the comments, but we don't need them here
+    */
+    newtio.c_cc[VINTR]    = 0;     /* Ctrl-c */ 
+    newtio.c_cc[VQUIT]    = 0;     /* Ctrl-\ */
+    newtio.c_cc[VERASE]   = 0;     /* del */
+    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[VSWTC]    = 0;     /* '\0' */
+    newtio.c_cc[VSTART]   = 0;     /* Ctrl-q */ 
+    newtio.c_cc[VSTOP]    = 0;     /* Ctrl-s */
+    newtio.c_cc[VSUSP]    = 0;     /* Ctrl-z */
+    newtio.c_cc[VEOL]     = 0;     /* '\0' */
+    newtio.c_cc[VREPRINT] = 0;     /* Ctrl-r */
+    newtio.c_cc[VDISCARD] = 0;     /* Ctrl-u */
+    newtio.c_cc[VWERASE]  = 0;     /* Ctrl-w */
+    newtio.c_cc[VLNEXT]   = 0;     /* Ctrl-v */
+    newtio.c_cc[VEOL2]    = 0;     /* '\0' */
+
+    /*
+      now clean the modem line and activate the settings for the port
+    */
+    tcflush(fd, TCIFLUSH);
+    tcsetattr(fd,TCSANOW,&newtio);
+
+    return fd;
+}
+
+void* loop_gnss_device(void* dev)
+{
+    int* p_fd = (int*)dev;
+    int fd = *p_fd;
+    fd_set readfs;    /* file descriptor set */
+    int    maxfd;     /* maximum file descriptor used */
+    int linecount=0;
+    char buf[MAX_GNSS_BUFFER_SIZE];
+    //read failure - used to trigger restart
+    bool read_failure = false;
+    //trigger message
+    NMEA_RESULT trigger = NMEA_RMC;
+    //gnss data as returned by NMEA parser
+    GNS_DATA gns_data;
+    HNMEA_Init_GNS_DATA(&gns_data);
+
+    /* loop until we have a terminating condition */
+    //LOG_DEBUG(gContext, "entering NMEA reading loop %d\n", fd);
+    while (g_gnss_loop)
+    {     
+        int res;
+        struct timeval timeout;
+        /* set timeout value within input loop */
+        timeout.tv_usec = 0;  /* milliseconds */
+        timeout.tv_sec  = 2;  /* seconds */
+        FD_SET(fd, &readfs);
+        maxfd = fd+1;
+
+        /* block until input becomes available */
+        res = select(maxfd, &readfs, NULL, NULL, &timeout);
+        if (res==-1)
+        {
+            read_failure = true;
+        }
+        else if (res==0)
+        {
+            //LOG_DEBUG_MSG(gContext, "TIMEOUT\n");
+        } 
+        else if (FD_ISSET(fd, &readfs))
+        {
+            res = read(fd,buf,MAX_GNSS_BUFFER_SIZE);
+            if (res == 0)
+            {
+                read_failure = true;
+            }
+            buf[res]=0;             /* set end of string, so we can printf */
+            linecount++;
+            //LOG_DEBUG(gContext, "%d:%s", linecount, buf);
+            NMEA_RESULT nmea_res = HNMEA_Parse(buf, &gns_data);
+
+            //most receivers sent GPRMC as last, but u-blox send as first: use other trigger
+            //determine most suitable trigger on actually received messages
+            if (nmea_res == NMEA_RMC)  //highest precedence
+            {
+                pthread_mutex_lock(&mutex_gnss);       /* down semaphore */
+                strncpy(gnssBuffer,buf,res);
+                gnssDataReady=true;
+                pthread_mutex_unlock(&mutex_gnss);       /* up semaphore */
+            }
+        }
+        if(read_failure)
+        {
+            //Error - try to restart device connection
+            close(fd);
+            fd = -1;
+            int device_open_retries = 0;
+            while ((device_open_retries < OPEN_RETRY_MAX) && (fd < 0))
+            {
+                device_open_retries++;
+                sleep(OPEN_RETRY_DELAY);
+                fd = gnss_open_device(g_gnss_device, g_baudrate);
+            }
+            if (fd >=0)
+            {
+                read_failure = false;
+            }
+            else
+            {
+                //reopen failed: terminate thread
+                g_gnss_loop = 0;
+            }
+        }
+    }
+    close(fd);
+    return NULL;
+}
+
+bool gnss_send_command(const char* cmd)
+{
+    if (write(g_gnss_fd,cmd,sizeof(cmd)-1))
+        return true;
+    else
+        return false;
+}
+
+bool gnss_init(const char* gnss_device, unsigned int baudrate)
+{
+    bool retval = false;
+    g_gnss_device = (char*)gnss_device;
+    g_baudrate = baudrate;
+    g_gnss_fd = gnss_open_device((char*)gnss_device, baudrate);
+    if (g_gnss_fd >= 0)
+    {
+        if(gnss_send_command(ACTIVATE_GST))
+            if(gnss_send_command(ACTIVATE_GRS)){
+                pthread_create(&g_thread_gnss, NULL, loop_gnss_device, &g_gnss_fd);
+                retval = true;
+            }
+    }
+
+    return retval;
+}
+
+bool gnss_destroy()
+{
+    g_gnss_loop = 0;
+    pthread_join(g_thread_gnss, NULL);
+
+    return true;
+}
diff --git a/src/vehicle-gateway/gnss.h b/src/vehicle-gateway/gnss.h
new file mode 100644
index 0000000..99a8883
--- /dev/null
+++ b/src/vehicle-gateway/gnss.h
@@ -0,0 +1,55 @@
+/**
+* @licence app begin@
+* SPDX-License-Identifier: MPL-2.0
+*
+* \copyright Copyright (C) 2017, PSA GROUP
+*
+* \file obd2.c
+*
+* \brief This file is part of the FSA project.
+*
+* \author Philippe Colliot <philippe.colliot@mpsa.com>
+*
+* \version 0.1
+*
+*
+* @ref http://tldp.org/HOWTO/Serial-Programming-HOWTO/x115.html
+*
+* Part of this code has been inspired by Helmut Schmidt <https://github.com/huirad>
+*
+*
+* This Source Code Form is subject to the terms of the
+* Mozilla Public License (MPL), 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/.
+*
+* For further information see http://www.genivi.org/.
+*
+* List of changes:
+* <date>, <name>, <description of change>
+*
+* @licence end@
+*/
+
+#ifndef INCLUDE_GNSS
+#define INCLUDE_GNSS
+#include <semaphore.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//shared data management
+#define MAX_GNSS_BUFFER_SIZE 255
+extern pthread_mutex_t mutex_gnss;
+extern bool gnssDataReady;
+extern char gnssBuffer[];
+
+bool gnss_init(const char* gnss_device, unsigned int baudrate);
+bool gnss_destroy();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/vehicle-gateway/hnmea.cpp b/src/vehicle-gateway/hnmea.cpp
new file mode 100644
index 0000000..9778eb1
--- /dev/null
+++ b/src/vehicle-gateway/hnmea.cpp
@@ -0,0 +1,916 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \brief Simple NMEA Parser for GPS data
+*        Original version: \see http://sourceforge.net/projects/get201/
+* 
+* \author Helmut Schmidt <https://github.com/huirad>
+*
+* \copyright Copyright (C) 2009, Helmut Schmidt
+* 
+* \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@
+**************************************************************************/
+
+
+#include "hnmea.h"
+#include "string.h"
+#include "stdlib.h"
+#include "math.h"
+
+//some example/test strings
+char test_gprmc[] = "$GPRMC,112911.000,A,4856.3328,N,01146.8259,E,35.75,108.51,050807,,,A*57";
+char test_gpgga[] = "$GPGGA,112911.000,4856.3328,N,01146.8259,E,1,06,2.0,353.1,M,47.5,M,,0000*50";
+char test_gpgsa[] = "$GPGSA,A,3,27,28,10,29,08,26,,,,,,,3.2,2.0,2.5*3F";
+char test_gpgs1[] = "$GPGSV,3,1,12,27,17,075,20,19,07,027,,21,05,296,,18,11,325,*77";
+char test_gpgs2[] = "$GPGSV,3,2,12,28,62,095,44,10,32,199,30,29,79,302,40,08,40,067,43*71";
+char test_gpgs3[] = "$GPGSV,3,3,12,09,08,259,29,26,65,304,45,24,02,263,,17,08,134,28*7E";
+
+
+
+void HNMEA_Init_GNS_DATA(GNS_DATA* gns_data)
+{
+    gns_data->valid         = 0;
+    gns_data->valid_new     = 0;
+    gns_data->valid_ext     = 0;
+    gns_data->valid_ext_new = 0;    
+    gns_data->lat           = 999.99;
+    gns_data->lon           = 999.99;
+    gns_data->alt           = -1000.0;
+    gns_data->geoid         = -1000.0;
+    gns_data->date_yyyy     = -1;
+    gns_data->date_mm       = -1;
+    gns_data->date_dd       = -1;
+    gns_data->time_hh       = -1;
+    gns_data->time_mm       = -1;
+    gns_data->time_ss       = -1;
+    gns_data->time_ms       = -1;
+    gns_data->course        = -999.99;
+    gns_data->speed         = -999.99;
+    gns_data->hdop          = -999.99;
+    gns_data->vdop          = -999.99;
+    gns_data->pdop          = -999.99;
+    gns_data->usat          = -99;
+    gns_data->fix2d         = -1;
+    gns_data->fix3d         = -1;
+    gns_data->hacc          = 999.9;
+    gns_data->vacc          = 999.9;
+    gns_data->usat_gps      = -99;
+    gns_data->usat_glo      = -99;    
+}
+
+
+//Test if the NMEA Checkum is valid. 
+//If no checkum is available, it is considered as valid
+//The optional checksum field consists of a "*" and two hex digits
+//  representing the exclusive OR of all characters between, but not
+//  including, the "$" and "*".
+int HNMEA_Checksum_Valid(char* line)
+{
+    int ret = 0;
+    char calc_checksum = 0;
+    char nmea_checksum = 0;
+    int i = 1; 
+    int len = strlen(line);
+
+    if ( (len > 1) || line[0] == '$')
+    {
+        //calculate checksum
+        while ( (i < len-1) && (line[i] != '*') )
+        {
+            calc_checksum = calc_checksum ^ line[i];
+            i++;
+        }
+        if ( (len >= i+3) && (line[i] == '*') )
+        {
+            //optionally, also strtoul() could be used for checksum reading
+            char str [2];
+            str[1] = 0;
+            
+            str[0] = line[i+1];
+            int c1 = strcspn("_0123456789ABCDEF", str);
+
+            str[0] = line[i+2];
+            int c2 = strcspn("_0123456789ABCDEF", str);
+
+            if ( (c1 > 0) && (c2 > 0) )
+            {
+                nmea_checksum = (c1-1)*16+(c2-1);
+                if (nmea_checksum == calc_checksum)
+                {
+                    ret = 1;
+                }
+            }
+        }
+        else //no checksum considered as valid
+        {
+            ret = 1;
+        }
+    }
+    return ret;
+}
+
+void HNMEA_Parse_RMC(char* line, GNS_DATA* gns_data)
+{
+    enum { MAX_FIELD_LEN = 128};
+    char field[MAX_FIELD_LEN];
+    int i = 0;      // field index
+    int l = 0;      // line character index
+    int f = 0;      // field character index
+    int stop = 0;   // stop flag
+    int len = strlen(line);
+
+    gns_data->valid_new = 0;
+    gns_data->valid_ext_new = 0;
+
+    //outer loop - stop at line end
+    while ( (l < len ) && (stop == 0) )
+    {
+        //inner loop - stop at line and and field separator
+        while ( (f < MAX_FIELD_LEN) && (l< len) && (line[l] != ',') && (line[l] != '*') )
+        {
+            field[f] = line[l];
+            l++;
+            f++;
+        }
+        field[f] = '\0'; // add string terminator
+
+        switch (i)
+        {
+            case 0: //$GPRMC or $GNRMC
+            {
+                //cross-check for sentence name
+                if ((strncmp (field, "$GPRMC", 6) != 0) && (strncmp (field, "$GNRMC", 6) != 0))
+                {
+                    // force termination of loop
+                    stop = 1;
+                }
+                break;
+            }
+            case 1: //time hhmmss.sss
+            {
+                //length check
+                if (strlen (field) >=6)
+                {
+                    gns_data->time_ss = atoi(field+4);
+                    gns_data->time_ms = (atof(field+4)-gns_data->time_ss)*1000;
+                    field[4] = '\0';
+                    gns_data->time_mm = atoi(field+2);
+                    field[2] = '\0';
+                    gns_data->time_hh = atoi(field);
+                    gns_data->valid_new |= GNS_DATA_TIME;
+                }
+                break;
+            }
+            case 2: //status - A = OK, V = warning
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    if (field[0] == 'A')
+                    {
+                        gns_data->fix2d = 1;
+                    }
+                    else
+                    {
+                        gns_data->fix2d = 0;
+                    }
+                    gns_data->valid_new |= GNS_DATA_FIX2D;
+                }
+                break;
+            }
+            case 3: //latitude - absolute value
+            {
+                //check for minimum length + evaluate only if status ok
+                if ( ((gns_data->valid_new & GNS_DATA_FIX2D)!=0) && (gns_data->fix2d) && (strlen (field) >=2) )
+                {
+                    double fraction = 0.0;
+                    if (strlen (field) >=3)
+                    {
+                        fraction = atof(field+2)/60.0;
+                    }
+                    field[2]=0;
+                    gns_data->lat = atoi(field) + fraction;
+                    gns_data->valid_new |= GNS_DATA_LAT;
+                }
+                break;
+            }
+            case 4: //latitude - sign
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    if ((field[0] == 'S') || (field[0] == 's'))
+                    {
+                        gns_data->lat = - gns_data->lat;
+                    }
+                }
+                break;
+            }
+            case 5: //longitude - absolute value
+            {
+                //check for minimum length + evaluate only if status ok
+                if ( ((gns_data->valid_new & GNS_DATA_FIX2D)!=0) && (gns_data->fix2d) && (strlen (field) >=3) )
+                {
+                    double fraction = 0.0;
+                    if (strlen (field) >=4)
+                    {
+                        fraction = atof(field+3)/60.0;
+                    }
+                    field[3]=0;
+                    gns_data->lon = atoi(field) + fraction;
+                    gns_data->valid_new |= GNS_DATA_LON;
+                }
+                break;
+            }
+            case 6: //longitude - sign
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    if ((field[0] == 'W') || (field[0] == 'w'))
+                    {
+                        gns_data->lon = - gns_data->lon;
+                    }
+                }
+                break;
+            }
+            case 7: //speed - knots
+            {
+                //length check + evaluate only if status ok
+                if (((gns_data->valid_new & GNS_DATA_FIX2D)!=0) && (gns_data->fix2d) && (strlen (field) >=1) )
+                {
+                    gns_data->speed = atof(field)*1.852/3.6;
+                    gns_data->valid_new |= GNS_DATA_SPEED;
+                }
+                break;
+            }
+            case 8: //course - degrees
+            {
+                //length check + evaluate only if status ok
+                if (((gns_data->valid_new & GNS_DATA_FIX2D)!=0) && (gns_data->fix2d) && (strlen (field) >=1) )
+                {
+                    gns_data->course = atof(field);
+                    gns_data->valid_new |= GNS_DATA_COURSE;
+                }
+                break;
+            }
+            case 9: //date yymmdd
+            {
+                //length check
+                if (strlen (field) >=6)
+                {
+                    gns_data->date_yyyy = 2000 + atoi(field+4);
+                    field[4] = '\0';
+                    gns_data->date_mm = atoi(field+2);
+                    field[2] = '\0';
+                    gns_data->date_dd = atoi(field);
+                    gns_data->valid_new |= GNS_DATA_DATE;
+                }
+                stop = 1; //ignore all other fields
+                break;
+            }
+            default:
+            {
+                stop = 1;
+                break;
+            }
+        }
+
+        //one more field?
+        if ( line[l] == ',' )
+        {
+            //skip separator
+            l++;
+            //reset 
+            f = 0;
+            //increment field index
+            i++;
+        }
+        else 
+        {
+            // force termination of loop
+            stop = 1;
+        }
+    }
+
+    //update validity mask with new data
+    gns_data->valid |= gns_data->valid_new;
+    gns_data->valid_ext |= gns_data->valid_ext_new;
+}
+
+void HNMEA_Parse_GGA(char* line, GNS_DATA* gns_data)
+{
+    enum { MAX_FIELD_LEN = 128};
+    char field[MAX_FIELD_LEN];
+    int i = 0;      // field index
+    int l = 0;      // line character index
+    int f = 0;      // field character index
+    int stop = 0;   // stop flag
+    int len = strlen(line);
+
+    //intermediate storage for lat, lon until Position Fix Indicator is evaluated
+    double lat = 0.0;
+    double lon = 0.0;
+    //intermediate storage for alt, geoid until units are correct
+    double alt = 0.0;
+    double geoid = 0.0;
+
+    gns_data->valid_new = 0;
+    gns_data->valid_ext_new = 0;
+
+    //outer loop - stop at line end
+    while ( (l < len ) && (stop == 0) )
+    {
+        //inner loop - stop at line and and field separator
+        while ( (f < MAX_FIELD_LEN) && (l< len) && (line[l] != ',') && (line[l] != '*') )
+        {
+            field[f] = line[l];
+            l++;
+            f++;
+        }
+        field[f] = '\0'; // add string terminator
+
+        switch (i)
+        {
+            case 0: //$GPGGA or $GNGGA
+            {
+                //cross-check for sentence name
+                if ((strncmp (field, "$GPGGA", 6) != 0) && (strncmp (field, "$GNGGA", 6) != 0))
+                {
+                    // force termination of loop
+                    stop = 1;
+                }
+                break;
+            }
+            case 1: //time hhmmss.sss
+            {
+                //length check
+                if (strlen (field) >=6)
+                {
+                    gns_data->time_ss = atoi(field+4);
+                    gns_data->time_ms = (atof(field+4)-gns_data->time_ss)*1000;
+                    field[4] = '\0';
+                    gns_data->time_mm = atoi(field+2);
+                    field[2] = '\0';
+                    gns_data->time_hh = atoi(field);
+                    gns_data->valid_new |= GNS_DATA_TIME;
+                }
+                break;
+            }
+            case 2: //latitude - absolute value
+            {
+                //check for minimum length
+                if (strlen (field) >=2)
+                {
+                    double fraction = 0.0;
+                    if (strlen (field) >=3)
+                    {
+                        fraction = atof(field+2)/60.0;
+                    }
+                    field[2]=0;
+                    lat = atoi(field) + fraction;
+                    gns_data->valid_new |= GNS_DATA_LAT;
+                }
+                break;
+            }
+            case 3: //latitude - sign
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    if ((field[0] == 'S') || (field[0] == 's'))
+                    {
+                        lat = - lat;
+                    }
+                }
+                break;
+            }
+            case 4: //longitude - absolute value
+            {
+                //check for minimum length
+                if (strlen (field) >=3)
+                {
+                    double fraction = 0.0;
+                    if (strlen (field) >=4)
+                    {
+                        fraction = atof(field+3)/60.0;
+                    }
+                    field[3]=0;
+                    lon = atoi(field) + fraction;
+                    gns_data->valid_new |= GNS_DATA_LON;
+                }
+                break;
+            }
+            case 5: //longitude - sign
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    if ((field[0] == 'W') || (field[0] == 'w'))
+                    {
+                        lon = - lon;
+                    }
+                }
+                break;
+            }
+            case 6: //position fix indicator
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    if ( (field[0] == '1') || (field[0] == '2') || (field[0] == '6') )
+                    {
+                        gns_data->fix2d = 1;
+                        gns_data->lat = lat;
+                        gns_data->lon = lon;
+                    }
+                    else
+                    {
+                        gns_data->fix2d = 0;
+                        gns_data->valid_new &= ~(GNS_DATA_LAT | GNS_DATA_LON);
+                    }
+                    gns_data->valid_new |= GNS_DATA_FIX2D;
+                }
+                break;
+            }
+            case 7: //number of used satellites
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    gns_data->usat = atoi(field);
+                    gns_data->valid_new |= GNS_DATA_USAT;
+                }
+                break;
+            }
+            case 8: //hdop
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    gns_data->hdop = atof(field);
+                    gns_data->valid_new |= GNS_DATA_HDOP;
+                }
+                break;
+            }
+            case 9: //altitude
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    alt = atof(field);
+                    gns_data->valid_new |= GNS_DATA_ALT;
+                }
+                break;
+            }
+            case 10: //altitude unit
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    if ( (field[0] == 'M') && (gns_data->fix2d) )
+                    {
+                        gns_data->alt = alt;
+                    }
+                    else
+                    {
+                        gns_data->valid_new &= ~(GNS_DATA_ALT);
+                    }
+                }
+                break;
+            }
+            case 11: //geoid separation
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    geoid = atof(field);
+                    gns_data->valid_new |= GNS_DATA_GEOID;
+                }
+                break;
+            }
+            case 12: //geoid separation unit
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    if (field[0] == 'M') 
+                    {
+                        gns_data->geoid = geoid;
+                    }
+                    else
+                    {
+                        gns_data->valid_new &= ~(GNS_DATA_GEOID);
+                    }
+                }
+                stop = 1; //ignore all other fields
+                break;
+            }
+            default:
+            {
+                stop = 1;
+                break;
+            }
+        }
+
+        //one more field?
+        if ( line[l] == ',' )
+        {
+            //skip separator
+            l++;
+            //reset 
+            f = 0;
+            //increment field index
+            i++;
+        }
+        else 
+        {
+            // force termination of loop
+            stop = 1;
+        }
+    }
+
+    //update validity mask with valid_new data
+    gns_data->valid |= gns_data->valid_new;
+    gns_data->valid_ext |= gns_data->valid_ext_new;
+
+}
+
+void HNMEA_Parse_GSA(char* line, GNS_DATA* gns_data)
+{
+    enum { MAX_FIELD_LEN = 128};
+    char field[MAX_FIELD_LEN];
+    int i = 0;      // field index
+    int l = 0;      // line character index
+    int f = 0;      // field character index
+    int stop = 0;   // stop flag
+    int len = strlen(line);
+
+    int usat = 0; //counter for used satellites
+    //NMEA 4.1: GSA has additional field systemId after VDOP
+    //1=GPS 2=GLONASS 3=Galileo 4=BeiDou 
+    int systemId = 0;
+
+    gns_data->valid_new = 0;
+    gns_data->valid_ext_new = 0;
+
+    //outer loop - stop at line end
+    while ( (l < len ) && (stop == 0) )
+    {
+        //inner loop - stop at line and and field separator
+        while ( (f < MAX_FIELD_LEN) && (l< len) && (line[l] != ',') && (line[l] != '*') )
+        {
+            field[f] = line[l];
+            l++;
+            f++;
+        }
+        field[f] = '\0'; // add string terminator
+
+        switch (i)
+        {
+            case 0: //$GPGSA or $GNGSA
+            {
+                //cross-check for sentence name
+                if ((strncmp (field, "$GPGSA", 6) != 0) && (strncmp (field, "$GNGSA", 6) != 0))
+                {
+                    // force termination of loop
+                    stop = 1;
+                }
+                break;
+            }
+            case 1: //selection mode - ignore
+            {
+                break;
+            }
+            case 2: //fix status 1- no fix, 2 - 2d fix, 3 - 3d fix
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    if (field[0] == '2')
+                    {
+                        gns_data->fix2d = 1;
+                        gns_data->fix3d = 0;
+                    }
+                    else if (field[0] == '3')
+                    {
+                        gns_data->fix2d = 1;
+                        gns_data->fix3d = 1;
+                    }
+                    else
+                    {
+                        gns_data->fix2d = 0;
+                        gns_data->fix3d = 0;
+                    }
+                    gns_data->valid_new |= GNS_DATA_FIX2D;
+                    gns_data->valid_new |= GNS_DATA_FIX3D;
+                }
+                break;
+            }
+            case 3: //sat id 1-12
+            case 4:
+            case 5:
+            case 6:
+            case 7:
+            case 8:
+            case 9:
+            case 10:
+            case 11:
+            case 12:
+            case 13:
+            case 14:
+            {
+                if (strlen (field) >=1)
+                {
+                    usat++;
+                }
+                break;
+            }
+            case 15: //PDOP
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    gns_data->pdop = atof(field);
+                    gns_data->valid_new |= GNS_DATA_PDOP;
+                }
+                break;
+            }
+            case 16: //HDOP
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    gns_data->hdop = atof(field);
+                    gns_data->valid_new |= GNS_DATA_HDOP;
+                }
+                break;
+            }
+            case 17: //VDOP
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    gns_data->vdop = atof(field);
+                    gns_data->valid_new |= GNS_DATA_VDOP;
+                }
+                break;
+            }
+            case 18: //NMEA 4.1 systemId
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    systemId = atoi(field);
+                }
+                stop = 1; //ignore all other fields
+                break;
+            }            
+            default:
+            {
+                stop = 1;
+                break;
+            }
+        }
+
+        //one more field?
+        if ( line[l] == ',' )
+        {
+            //skip separator
+            l++;
+            //reset 
+            f = 0;
+            //increment field index
+            i++;
+        }
+        else 
+        {
+            // force termination of loop
+            stop = 1;
+        }
+    }
+
+    if (usat > 0)
+    {
+        if (systemId == 0) //unspecified
+        {
+            gns_data->usat = usat;
+            gns_data->valid_new |= GNS_DATA_USAT;
+        }
+        else if (systemId == 1) //GPS
+        {
+            gns_data->usat_gps = usat;
+            gns_data->valid_ext_new |= GNS_DATA_USAT_GPS;
+        }
+        else if (systemId == 2) //GLONASS
+        {
+            gns_data->usat_glo = usat;
+            gns_data->valid_ext_new |= GNS_DATA_USAT_GLO;
+        }
+    }
+
+    //update validity mask with new data
+    gns_data->valid |= gns_data->valid_new;
+    gns_data->valid_ext |= gns_data->valid_ext_new;
+}
+
+void HNMEA_Parse_GST(char* line, GNS_DATA* gns_data)
+{
+    enum { MAX_FIELD_LEN = 128};
+    char field[MAX_FIELD_LEN];
+    int i = 0;      // field index
+    int l = 0;      // line character index
+    int f = 0;      // field character index
+    int stop = 0;   // stop flag
+    int len = strlen(line);
+
+    
+    float lat_std = 0.0;
+    int lat_std_valid = 0;
+    float lon_std = 0.0;
+    float alt_std = 0.0;
+    
+    gns_data->valid_new = 0;
+    gns_data->valid_ext_new = 0;
+
+    //outer loop - stop at line end
+    while ( (l < len ) && (stop == 0) )
+    {
+        //inner loop - stop at line and and field separator
+        while ( (f < MAX_FIELD_LEN) && (l< len) && (line[l] != ',') && (line[l] != '*') )
+        {
+            field[f] = line[l];
+            l++;
+            f++;
+        }
+        field[f] = '\0'; // add string terminator
+
+        switch (i)
+        {
+            case 0: //$GPGST
+            {
+                //cross-check for sentence name
+                if ((strncmp (field, "$GPGST", 6) != 0) && (strncmp (field, "$GNGST", 6) != 0))
+                {
+                    // force termination of loop
+                    stop = 1;
+                }
+                break;
+            }
+            case 1: //time hhmmss.sss
+            {
+                //length check
+                if (strlen (field) >=6)
+                {
+                    gns_data->time_ss = atoi(field+4);
+                    gns_data->time_ms = (atof(field+4)-gns_data->time_ss)*1000;
+                    field[4] = '\0';
+                    gns_data->time_mm = atoi(field+2);
+                    field[2] = '\0';
+                    gns_data->time_hh = atoi(field);
+                    gns_data->valid_new |= GNS_DATA_TIME;
+                }
+                break;
+            }
+            case 2: // RMS value of the standard deviation of the ranges
+            {
+                //ignore
+                break;
+            }
+            case 3: //Standard deviation of semi-major axis,
+            {
+                //ignore
+                break;
+            }
+            case 4: //Standard deviation of semi-minor axis
+            {
+                //ignore
+                break;
+            }
+            case 5: //Orientation of semi-major axis
+            {
+                //ignore
+                break;
+            }
+            case 6: //Standard deviation of latitude, error in meters
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    lat_std = atof(field);
+                    lat_std_valid = 1;
+                }
+                break;
+            }
+            case 7: //Standard deviation of longitude, error in meters
+            {
+                //length check
+                if ((strlen (field) >=1) && (lat_std_valid))
+                {
+                    lon_std = atof(field);
+                    gns_data->hacc = sqrt(lat_std*lat_std + lon_std*lon_std);
+                    gns_data->valid_new |= GNS_DATA_HACC;
+                }
+                break;
+            }
+            case 8: //Standard deviation of altitude, error in meters
+            {
+                //length check
+                if (strlen (field) >=1)
+                {
+                    alt_std = atof(field);
+                    gns_data->vacc = alt_std;
+                    gns_data->valid_new |= GNS_DATA_VACC;
+                }
+                break;
+            }
+            default:
+            {
+                stop = 1;
+                break;
+            }
+        }
+
+        //one more field?
+        if ( line[l] == ',' )
+        {
+            //skip separator
+            l++;
+            //reset 
+            f = 0;
+            //increment field index
+            i++;
+        }
+        else 
+        {
+            // force termination of loop
+            stop = 1;
+        }
+    }
+
+    //update validity mask with new data
+    gns_data->valid |= gns_data->valid_new;
+    gns_data->valid_ext |= gns_data->valid_ext_new;
+}
+
+NMEA_RESULT HNMEA_Parse(char* line, GNS_DATA* gns_data)
+{
+    NMEA_RESULT ret = NMEA_UKNOWN;
+    if ((strncmp (line, "$GPRMC", 6) == 0) || (strncmp (line, "$GNRMC", 6) == 0))
+    {
+        if (HNMEA_Checksum_Valid(line))
+        {
+            HNMEA_Parse_RMC(line, gns_data);
+            ret = NMEA_RMC;
+        }
+        else
+        {
+            ret = NMEA_BAD_CHKSUM;
+        }
+    }
+    else if ((strncmp (line, "$GPGGA", 6) == 0) || (strncmp (line, "$GNGGA", 6) == 0))
+    {
+        if (HNMEA_Checksum_Valid(line))
+        {
+            HNMEA_Parse_GGA(line, gns_data);
+            ret = NMEA_GGA;
+        }
+        else
+        {
+            ret = NMEA_BAD_CHKSUM;
+        }
+    }
+    else if ((strncmp (line, "$GPGSA", 6) == 0) || (strncmp (line, "$GNGSA", 6) == 0))
+    {
+        if (HNMEA_Checksum_Valid(line))
+        {
+            HNMEA_Parse_GSA(line, gns_data);
+            ret = NMEA_GSA;
+        }
+        else
+        {
+            ret = NMEA_BAD_CHKSUM;
+        }
+    }
+    else if ((strncmp (line, "$GPGST", 6) == 0) || (strncmp (line, "$GNGST", 6) == 0))
+    {
+        if (HNMEA_Checksum_Valid(line))
+        {
+            HNMEA_Parse_GST(line, gns_data);
+            ret = NMEA_GST;
+        }
+        else
+        {
+            ret = NMEA_BAD_CHKSUM;
+        }
+    }
+    return ret;
+}
+
diff --git a/src/vehicle-gateway/hnmea.h b/src/vehicle-gateway/hnmea.h
new file mode 100644
index 0000000..a73aefd
--- /dev/null
+++ b/src/vehicle-gateway/hnmea.h
@@ -0,0 +1,99 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \brief Simple NMEA Parser for GPS data
+*        Original version: \see http://sourceforge.net/projects/get201/
+* 
+* \author Helmut Schmidt <https://github.com/huirad>
+*
+* \copyright Copyright (C) 2009, Helmut Schmidt
+* 
+* \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 _HNMEA_H
+#define _HNMEA_H
+
+
+//enum for NMEA parser result
+typedef enum {
+    NMEA_INIT,          //initial value - will never be returned by the parser
+    NMEA_UKNOWN,        //unknown content
+    NMEA_BAD_CHKSUM,    //BAD NMEA Checksum
+    NMEA_RMC,           //GxRMC Sentence
+    NMEA_GGA,           //GxGGA Sentence
+    NMEA_GSA,           //GxGSA Sentence
+    NMEA_GSV,           //GxGSV Sentence
+    NMEA_GST            //GxGST Sentence
+} NMEA_RESULT;
+
+//bitmaps for GNSS data
+typedef enum {
+    GNS_DATA_LAT    = 0x0001,  //latitude
+    GNS_DATA_LON    = 0x0002,  //longitude
+    GNS_DATA_ALT    = 0x0004,  //altitude
+    GNS_DATA_GEOID  = 0x0008,  //geoid separation
+    GNS_DATA_DATE   = 0x0010,  //date
+    GNS_DATA_TIME   = 0x0020,  //time
+    GNS_DATA_COURSE = 0x0040,  //course (heading)
+    GNS_DATA_SPEED  = 0x0080,  //speed
+    GNS_DATA_HDOP   = 0x0100,  //HDOP
+    GNS_DATA_VDOP   = 0x0200,  //VDOP
+    GNS_DATA_PDOP   = 0x0400,  //PDOP
+    GNS_DATA_USAT   = 0x0800,  //number of used satellites
+    GNS_DATA_FIX2D  = 0x1000,  //at least 2D Fix
+    GNS_DATA_FIX3D  = 0x2000,  //3D Fix (GPS_DATA_FIX2D will be set also)
+    GNS_DATA_HACC   = 0x4000,  //horizontal accuracy
+    GNS_DATA_VACC   = 0x8000   //vertical accuracy
+} GNS_DATA_TYPE;
+
+typedef enum {
+    GNS_DATA_USAT_GPS   = 0x0001,  //number of used GPS satellites
+    GNS_DATA_USAT_GLO   = 0x0002   //number of used GLONASS satellites    
+} GNS_DATA_EXT_TYPE;    
+
+typedef struct {
+    int valid;      //bitmask of GNS_DATA_TYPE for cumulative valid fields
+    int valid_new;  //bitmask of GNS_DATA_TYPE for new valid fields
+    int valid_ext;  //bitmask of GNS_DATA_EXT_TYPE for cumulative valid fields
+    int valid_ext_new;  //bitmask of GNS_DATA_EXT_TYPE for new valid fields    
+    double lat;     //latitude in degrees - negative values are south of equator
+    double lon;     //longitude in degrees - negative values are west of Greenwich
+    double alt;     //altitude in meter above sea level
+    double geoid;   //geoid separation in meter
+    int date_yyyy;  //date::year since B.C.
+    int date_mm;    //date::month - january = 1
+    int date_dd;    //date::day - day of month 
+    int time_hh;    //time::hour
+    int time_mm;    //time::minute
+    int time_ss;    //time::second
+    int time_ms;    //time::millisecond
+    double course;  //course (heading) in degrees compass-like
+    double speed;   //speed im m/s
+    float hdop;     //hdop
+    float vdop;     //vdop
+    float pdop;     //pdop
+    int usat;       //number of satellites
+    int fix2d;      //GPS status: at least 2D Fix
+    int fix3d;      //GPS status: 3D Fix - fix2d will be set also 
+    float hacc;     //horizontal accuracy in m
+    float vacc;     //vertical accuracy in m
+    int usat_gps;   //number of GPS satellites
+    int usat_glo;   //number of GLONASS satellites    
+} GNS_DATA;
+
+void HNMEA_Init_GNS_DATA(GNS_DATA* gns_data);
+
+NMEA_RESULT HNMEA_Parse(char* line, GNS_DATA* gns_data);
+
+#endif //_HNMEA_H
+
diff --git a/src/vehicle-gateway/log.h b/src/vehicle-gateway/log.h
new file mode 100644
index 0000000..fa5c3ca
--- /dev/null
+++ b/src/vehicle-gateway/log.h
@@ -0,0 +1,154 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \ingroup SensorsService
+*
+* \copyright  Copyright (C) BMW Car IT GmbH 2011
+*             Copyright (C) 2013, XS Embedded GmbH
+*
+* \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_LOG
+#define INCLUDE_LOG
+
+// turn-on via cmake define:
+// $ cmake -DWITH_DLT=1 -DDEBUG_ENABLED=1 ..
+
+#if (!DLT_ENABLED)
+/*****************************************************************************/
+// use printf
+#include <stdio.h>
+
+// some type-name used instead of DLT context
+typedef const char* NoDltContext;
+
+#define DLT_DECLARE_CONTEXT(CONTEXT) \
+    NoDltContext CONTEXT;
+
+#define DLT_IMPORT_CONTEXT(CONTEXT) \
+    extern NoDltContext CONTEXT;
+
+#define DLT_REGISTER_CONTEXT(CONTEXT, CONTEXT_ID, DESC) \
+    CONTEXT = CONTEXT_ID;
+
+#define DLT_REGISTER_APP(CONTEXT, DESC) ;
+
+#define DLT_UNREGISTER_CONTEXT(CONTEXT) ;
+#define DLT_UNREGISTER_APP() ;
+#define dlt_free() ;
+
+// log calls
+#if (!DEBUG_ENABLED)
+
+#define LOG_VERBOSE_MSG(context, msg) ;
+#define LOG_VERBOSE(context, fmt, ...) ;
+
+#define LOG_DEBUG_MSG(context, msg) ;
+#define LOG_DEBUG(context, fmt, ...) ;
+
+#define LOG_INFO_MSG(context, msg) ;
+#define LOG_INFO(context, fmt, ...) ;
+
+#define LOG_WARNING_MSG(context, msg) ;
+#define LOG_WARNING(context, fmt, ...) ;
+
+#else
+
+#define LOG_VERBOSE_MSG(context, msg) \
+        fprintf(stderr, "[VERBO][%4s] " msg "\n", context)
+#define LOG_VERBOSE(context, fmt, ...) \
+        fprintf(stderr, "[VERBO][%4s] " fmt "\n", context, __VA_ARGS__)
+
+#define LOG_DEBUG_MSG(context, msg) \
+        fprintf(stderr, "[DEBUG][%4s] " msg "\n", context)
+#define LOG_DEBUG(context, fmt, ...) \
+        fprintf(stderr, "[DEBUG][%4s] " fmt "\n", context, __VA_ARGS__)
+
+#define LOG_INFO_MSG(context, msg) \
+        fprintf(stderr, "[INFO ][%4s] " msg "\n", context)
+#define LOG_INFO(context, fmt, ...) \
+        fprintf(stderr, "[INFO ][%4s] " fmt "\n", context, __VA_ARGS__)
+
+#define LOG_WARNING_MSG(context, msg) \
+        fprintf(stderr, "[WARN ][%4s] " msg "\n", context)
+#define LOG_WARNING(context, fmt, ...) \
+        fprintf(stderr, "[WARN ][%4s] " fmt "\n", context, __VA_ARGS__)
+#endif
+
+#define LOG_ERROR_MSG(context, msg) \
+        fprintf(stderr, "[ERROR][%4s] " msg "\n", context)
+#define LOG_ERROR(context, fmt, ...) \
+        fprintf(stderr, "[ERROR][%4s] " fmt "\n", context, __VA_ARGS__)
+
+#define LOG_FATAL_MSG(context, msg) \
+        fprintf(stderr, "[FATAL][%4s] " msg "\n", context)
+#define LOG_FATAL(context, fmt, ...) \
+        fprintf(stderr, "[FATAL][%4s] " fmt "\n", context, __VA_ARGS__)
+
+#else   /* DLT_ENABLED */
+/*****************************************************************************/
+// use DLT
+#include "dlt.h"
+
+typedef const char* Context;
+
+#define LOG_VERBOSE_MSG(context, msg) \
+        DLT_LOG(context, DLT_LOG_VERBOSE, DLT_STRING(msg));
+#define LOG_VERBOSE(context, fmt, ...) \
+        { \
+            char logBuffer[256]; \
+            sprintf(logBuffer, fmt, __VA_ARGS__); \
+            DLT_LOG(context, DLT_LOG_VERBOSE, DLT_STRING(logBuffer)); \
+        }
+#define LOG_DEBUG_MSG(context, msg) \
+        DLT_LOG(context, DLT_LOG_DEBUG, DLT_STRING(msg));
+#define LOG_DEBUG(context, fmt, ...) \
+        { \
+            char logBuffer[256]; \
+            sprintf(logBuffer, fmt, __VA_ARGS__); \
+            DLT_LOG(context, DLT_LOG_DEBUG, DLT_STRING(logBuffer)); \
+        }
+#define LOG_INFO_MSG(context, msg) \
+        DLT_LOG(context, DLT_LOG_INFO, DLT_STRING(msg));
+#define LOG_INFO(context, fmt, ...) \
+        { \
+            char logBuffer[256]; \
+            sprintf(logBuffer, fmt, __VA_ARGS__); \
+            DLT_LOG(context, DLT_LOG_INFO, DLT_STRING(logBuffer)); \
+        }
+#define LOG_WARNING_MSG(context, msg) \
+        DLT_LOG(context, DLT_LOG_WARN, DLT_STRING(msg));
+#define LOG_WARNING(context, fmt, ...) \
+        { \
+            char logBuffer[256]; \
+            sprintf(logBuffer, fmt, __VA_ARGS__); \
+            DLT_LOG(context, DLT_LOG_WARN, DLT_STRING(logBuffer)); \
+        }
+#define LOG_ERROR_MSG(context, msg) \
+        DLT_LOG(context, DLT_LOG_ERROR, DLT_STRING(msg));
+#define LOG_ERROR(context, fmt, ...) \
+        { \
+            char logBuffer[256]; \
+            sprintf(logBuffer, fmt, __VA_ARGS__); \
+            DLT_LOG(context, DLT_LOG_ERROR, DLT_STRING(logBuffer)); \
+        }
+#define LOG_FATAL_MSG(context, msg) \
+        DLT_LOG(context, DLT_LOG_FATAL, DLT_STRING(msg));
+#define LOG_FATAL(context, fmt, ...) \
+        { \
+            char logBuffer[256]; \
+            sprintf(logBuffer, fmt, __VA_ARGS__); \
+            DLT_LOG(context, DLT_LOG_FATAL, DLT_STRING(logBuffer)); \
+        }
+
+#endif  /* DLT_ENABLED */
+
+#endif /* INCLUDE_LOG */
diff --git a/src/vehicle-gateway/obd2.cpp b/src/vehicle-gateway/obd2.cpp
new file mode 100644
index 0000000..0ded44c
--- /dev/null
+++ b/src/vehicle-gateway/obd2.cpp
@@ -0,0 +1,208 @@
+/**
+* @licence app begin@
+* SPDX-License-Identifier: MPL-2.0
+*
+* \copyright Copyright (C) 2017, PSA GROUP
+*
+* \file obd2.c
+*
+* \brief This file is part of the FSA project.
+*
+* \author Philippe Colliot <philippe.colliot@mpsa.com>
+*
+* \version 0.1
+*
+*
+* @ref http://tldp.org/HOWTO/Serial-Programming-HOWTO/x115.html
+*
+* Part of this code has been inspired by Helmut Schmidt <https://github.com/huirad>
+*
+*
+* This Source Code Form is subject to the terms of the
+* Mozilla Public License (MPL), 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/.
+*
+* For further information see http://www.genivi.org/.
+*
+* List of changes:
+* <date>, <name>, <description of change>
+*
+* @licence end@
+*/
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <string.h>
+#include <time.h>
+#include <inttypes.h>
+
+#include <obd2.h>
+
+/* baudrate settings are defined in <asm/termbits.h>, which is
+included by <termios.h> */
+
+#define OBD_PID_FUEL_TANK 0x012F
+#define OBD_PID_RPM 0x010C
+#define OBD_PID_VEH_SPEED 0x010D
+
+#define ELM_RESET_ALL "AT Z\r\n"
+#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 CR '\r'
+#define EOS '\0'
+#define SPACE ' '
+#define BUFFER_MAX_LENGTH 512
+
+static int g_obd2_fd = -1;
+static struct termios g_oldtio;
+
+int obd2_open_device(char* obd2_device, unsigned int baudrate)
+{
+    int fd;
+    struct termios newtio;
+  /*
+    Open modem device for reading and writing and not as controlling tty
+    because we don't want to get killed if linenoise sends CTRL-C.
+  */
+   fd = open(obd2_device, O_RDWR | O_NOCTTY );
+   if (fd <0) {perror(obd2_device); return (-1); }
+
+   tcgetattr(fd,&g_oldtio); /* save current serial port settings */
+   bzero(&newtio, sizeof(newtio)); /* clear struct for new port settings */
+
+  /*
+    BAUDRATE: Set bps rate. You could also use cfsetispeed and cfsetospeed.
+    CRTSCTS : output hardware flow control (only used if the cable has
+              all necessary lines. See sect. 7 of Serial-HOWTO)
+    CS8     : 8n1 (8bit,no parity,1 stopbit)
+    CLOCAL  : local connection, no modem contol
+    CREAD   : enable receiving characters
+  */
+   newtio.c_cflag = baudrate | CS8 | CLOCAL | CREAD;
+
+  /*
+    IGNPAR  : ignore bytes with parity errors
+    ICRNL   : map CR to NL (otherwise a CR input on the other computer
+              will not terminate input)
+    otherwise make device raw (no other input processing)
+  */
+   newtio.c_iflag = IGNPAR;
+
+  /*
+   Raw output.
+  */
+   newtio.c_oflag = 0;
+
+  /*
+    ICANON  : enable canonical input
+    disable all echo functionality, and don't send signals to calling program
+  */
+   newtio.c_lflag = ISIG;
+
+  /*
+    initialize all control characters
+    default values can be found in /usr/include/termios.h, and are given
+    in the comments, but we don't need them here
+  */
+   newtio.c_cc[VINTR]    = 0;     /* Ctrl-c */
+   newtio.c_cc[VQUIT]    = 0;     /* Ctrl-\ */
+   newtio.c_cc[VERASE]   = 0;     /* del */
+   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[VSWTC]    = 0;     /* '\0' */
+   newtio.c_cc[VSTART]   = 0;     /* Ctrl-q */
+   newtio.c_cc[VSTOP]    = 0;     /* Ctrl-s */
+   newtio.c_cc[VSUSP]    = 0;     /* Ctrl-z */
+   newtio.c_cc[VEOL]     = 0;     /* '\0' */
+   newtio.c_cc[VREPRINT] = 0;     /* Ctrl-r */
+   newtio.c_cc[VDISCARD] = 0;     /* Ctrl-u */
+   newtio.c_cc[VWERASE]  = 0;     /* Ctrl-w */
+   newtio.c_cc[VLNEXT]   = 0;     /* Ctrl-v */
+   newtio.c_cc[VEOL2]    = 0;     /* '\0' */
+
+  /*
+    now clean the modem line and activate the settings for the port
+  */
+   tcflush(fd, TCIFLUSH);
+   tcsetattr(fd,TCSANOW,&newtio);
+
+   return fd;
+}
+
+bool obd2_read_answer(char*& ans,size_t* length)
+{ //ans is allocated dynamically
+    bool isRead=false;
+    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");
+                    break;
+                }else{
+                    if(buf==ELM_PROMPT){
+                        isRead=true;
+                        *(tmp+buf_length)=buf;
+                        ans = tmp;
+                        *length=buf_length;
+                    }
+                    else{
+                        if(buf==CR)
+                            buf=SPACE;
+                        *(tmp+buf_length)=buf;
+                    }
+                    buf_length++;
+                }
+            }
+        }
+        usleep(ELM_READ_LOOP);
+        timeout+=ELM_READ_LOOP;
+    }while((isRead==false)&&(timeout<ELM_READ_TIMEOUT));
+
+    return isRead;
+}
+
+bool obd2_send_command(const char* cmd)
+{
+    if (write(g_obd2_fd,cmd,sizeof(cmd)-1))
+        return true;
+    else
+        return false;
+}
+
+bool obd2_init(char* obd2_device, unsigned int baudrate)
+{
+    bool retval = false;
+
+    g_obd2_fd = obd2_open_device(obd2_device, baudrate);
+    if (g_obd2_fd >= 0)
+    {
+      retval = true;
+    }
+
+    return retval;
+}
+
+
+
diff --git a/src/vehicle-gateway/obd2.h b/src/vehicle-gateway/obd2.h
new file mode 100644
index 0000000..8c43560
--- /dev/null
+++ b/src/vehicle-gateway/obd2.h
@@ -0,0 +1,56 @@
+/**
+* @licence app begin@
+* SPDX-License-Identifier: MPL-2.0
+*
+* \copyright Copyright (C) 2017, PSA GROUP
+*
+* \file obd2.c
+*
+* \brief This file is part of the FSA project.
+*
+* \author Philippe Colliot <philippe.colliot@mpsa.com>
+*
+* \version 0.1
+*
+*
+* @ref http://tldp.org/HOWTO/Serial-Programming-HOWTO/x115.html
+*
+* Part of this code has been inspired by Helmut Schmidt <https://github.com/huirad>
+*
+*
+* This Source Code Form is subject to the terms of the
+* Mozilla Public License (MPL), 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/.
+*
+* For further information see http://www.genivi.org/.
+*
+* List of changes:
+* <date>, <name>, <description of change>
+*
+* @licence end@
+*/
+
+#ifndef INCLUDE_OBD2
+#define INCLUDE_OBD2
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+#include <sys/types.h>
+
+bool obd2_read_answer(char*& ans, size_t* length);
+
+bool obd2_send_command(const char* cmd);
+
+bool obd2_init(char* obd2_device, unsigned int baudrate);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/vehicle-gateway/veh-gateway.cpp b/src/vehicle-gateway/veh-gateway.cpp
new file mode 100644
index 0000000..9847fae
--- /dev/null
+++ b/src/vehicle-gateway/veh-gateway.cpp
@@ -0,0 +1,347 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \ingroup LogReplayer
+* \author Marco Residori <marco.residori@xse.de>
+*
+* \copyright Copyright (C) 2013, XS Embedded GmbH
+* 
+* \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@
+**************************************************************************/
+
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <stdio.h>
+#include <signal.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <memory.h>
+#include <time.h>
+#include <inttypes.h>
+#include <pthread.h>
+#include <semaphore.h>
+#include <termios.h>
+
+#include <obd2.h>
+#include <gnss.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
+const char * IPADDR_DEFAULT = "127.0.0.1";
+const char * GNS_PREFIX = "GVGNS";
+const char * SNS_PREFIX = "GVSNS";
+const char * VEH_PREFIX = "GVVEH";
+
+#define MAXDELTA 1000  //max value to avoid overflow
+
+#define BAUDRATE_OBD2 B38400
+#define ELM_RESET_ALL "AT Z\r\n"
+#define ELM_GET_ID "AT I\r\n"
+
+#define NMEA_TOKEN ","
+#define NMEA_RMC_STATUS 2
+#define NMEA_RMC_LATITUDE 3
+#define NMEA_RMC_LATITUDE_INDICATOR 4
+#define NMEA_RMC_LONGITUDE 5
+#define NMEA_RMC_LONGITUDE_INDICATOR 6
+#define NMEA_RMC_DATE 9
+#define NMEA_DATA_VALID "A"
+#define NMEA_WEST "W"
+#define NMEA_SOUTH "S"
+#define BAUDRATE_GNSS B38400
+
+typedef struct
+{
+    double latitude;
+    double longitude;
+    double altitude;
+} geocoordinate3D_t;
+
+DLT_DECLARE_CONTEXT(gContext);
+
+bool isRunning=true;
+
+static int g_obd2_fd = -1;
+static struct termios g_oldtio;
+
+uint64_t get_timestamp()
+{
+  struct timespec time_value;
+  if (clock_gettime(CLOCK_MONOTONIC, &time_value) != -1)
+  {
+    return (time_value.tv_sec*1000 + time_value.tv_nsec/1000000);
+  }
+  else
+  {
+    return 0xFFFFFFFFFFFFFFFF;
+  }
+}
+
+bool get_geolocation(char*& sock_buf,char* buffer)
+{
+    geocoordinate3D_t geolocation;
+    char* tmp = new char[BUFLEN];
+    char *token;
+    uint8_t cnt=0;
+    bool retval = false;
+    geolocation.latitude=0;
+    geolocation.longitude=0;
+    geolocation.altitude=0;
+    token=strtok(buffer,NMEA_TOKEN); //to load the pipe
+    cnt++;
+    while(token != NULL)
+    {
+        token=strtok(NULL,NMEA_TOKEN);
+        switch (cnt) {
+        case NMEA_RMC_STATUS:
+            if(strcmp(token,NMEA_DATA_VALID)==0){
+                retval=true;
+            }else{
+                return retval;
+            }
+            break;
+        case NMEA_RMC_LATITUDE:
+            geolocation.latitude=atof(token);
+            break;
+        case NMEA_RMC_LATITUDE_INDICATOR:
+            if(token==NMEA_SOUTH) geolocation.latitude=(-1)*geolocation.latitude;
+            break;
+        case NMEA_RMC_LONGITUDE:
+            geolocation.longitude=atof(token);
+            break;
+        case NMEA_RMC_LONGITUDE_INDICATOR:
+            if(token==NMEA_WEST) geolocation.longitude=(-1)*geolocation.longitude;
+            break;
+        case NMEA_RMC_DATE:
+            //to do
+            break;
+        default:
+            break;
+        }
+        cnt++;
+    }
+    LOG_DEBUG(gContext,"Lat: %f Lon: %f Alt: %f\n",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",1000,"0$GVGNSP",1000,geolocation.latitude,geolocation.longitude,geolocation.altitude);
+    sock_buf=tmp;
+    return retval;
+}
+
+void sighandler(int sig)
+{
+  LOG_INFO_MSG(gContext,"Signal received");
+  isRunning = false;
+}
+
+int main(int argc, char* argv[])
+{
+    // socket
+    struct sockaddr_in si_other;
+    socklen_t slen = sizeof(si_other);
+    int sock;
+    char* sock_buf;
+    char msgId[MSGIDLEN];
+    char * ipaddr = 0;
+
+    // OBD and GNSS devices
+    bool result;
+    uint64_t start, stop;
+    char* answer;
+    size_t answer_length;
+    char * modem_device_obd2 = 0;
+    char * modem_device_gnss = 0;
+    char* gnssprefix = (char*)GNS_PREFIX;
+    char* snsprefix = (char*)SNS_PREFIX;
+    char* vehprefix = (char*)VEH_PREFIX;
+    char gnss_buf[MAX_GNSS_BUFFER_SIZE];
+
+    // arguments check
+    if(argc < 3)
+    {
+       LOG_ERROR_MSG(gContext,"missing input parameters: ELM327DEVICE GNSSDEVICE");
+       return EXIT_FAILURE;
+    }
+    else
+    {
+        modem_device_obd2 = argv[1];
+        modem_device_gnss = argv[2];
+        if(argc < 4)
+            ipaddr = (char*)IPADDR_DEFAULT;
+        else
+            ipaddr = argv[3];
+    }
+
+
+    // 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);
+    signal(SIGINT, sighandler);
+    if((sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1)
+    {
+        LOG_ERROR_MSG(gContext,"socket() failed!");
+        return EXIT_FAILURE;
+    }
+
+    memset((char *) &si_other, 0, sizeof(si_other));
+    si_other.sin_family = AF_INET;
+    //si_other.sin_port = htons(<port number>);
+    if(inet_aton(ipaddr, &si_other.sin_addr) == 0)
+    {
+        LOG_ERROR_MSG(gContext,"inet_aton() failed!");
+        return EXIT_FAILURE;
+    }
+
+    LOG_INFO(gContext,"Started reading devices OBD2: %s GNSS: %s",modem_device_obd2, modem_device_gnss);
+
+    // connect to the devices
+    start = get_timestamp();
+    result = obd2_init(modem_device_obd2, BAUDRATE_OBD2);
+    stop = get_timestamp();
+    if (result)
+    {
+      LOG_INFO(gContext,"INIT OBD2 OK [DURATION = %" PRIu64 " ms]\n", 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);
+      return(-1);
+    }
+
+    gnssDataReady=false; //set the flag to false (no need of semaphore now because thread is not started yet)
+    start = get_timestamp();
+    result = gnss_init(modem_device_gnss, BAUDRATE_GNSS);
+    stop = get_timestamp();
+    if (result)
+    {
+      LOG_INFO(gContext,"INIT GNSS OK [DURATION = %" PRIu64 " ms]\n", 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);
+      return(-1);
+    }
+
+    // reset the OBD2 device
+    if (obd2_send_command(ELM_RESET_ALL)){
+        answer=NULL;
+        if(obd2_read_answer(answer,&answer_length)!=true){
+            LOG_ERROR_MSG(gContext,"RESET OBD2 FAILURE\n");
+            return(-1);
+        }
+    }else{
+        LOG_ERROR_MSG(gContext,"RESET OBD2 FAILURE\n");
+        return(-1);
+    }
+
+    //config the GNSS device to send given frames (TBD)
+
+    bool snsDataReady=false;
+    bool vehDataReady=false;
+    //main loop
+    do{
+        if (obd2_send_command(ELM_GET_ID)!=true){
+            LOG_DEBUG(gContext,"WRITE OBD2 FAILURE ON COMMAND: %s\n",ELM_GET_ID);
+            isRunning=false;
+        }
+
+        //GNSS: list of supported message IDs
+        //char* gnssstr = "GVGNSP,GVGNSC,GVGNSAC,GVGNSSAT";
+        pthread_mutex_lock(&mutex_gnss);       /* down semaphore */
+        if(gnssDataReady)
+        {
+            gnssDataReady=false;
+            strcpy(gnss_buf,gnssBuffer); //get the gnss buffer (end of string is added in gnss code)
+            pthread_mutex_unlock(&mutex_gnss);       /* up semaphore */
+            if(get_geolocation(sock_buf,gnss_buf))
+            {
+                LOG_DEBUG(gContext,"Sending Packet to %s:%d\n",ipaddr,PORT1);
+                LOG_DEBUG(gContext,"MsgID:%s\n", gnssprefix);
+                LOG_DEBUG(gContext,"Len:%d\n", (int)strlen(sock_buf));
+                LOG_DEBUG(gContext,"Data:%s\n", sock_buf);
+
+                si_other.sin_port = htons(PORT1);
+                if(sendto(sock, sock_buf, strlen(sock_buf)+1, 0, (struct sockaddr *)&si_other, slen) == -1)
+                {
+                    LOG_ERROR_MSG(gContext,"sendto() failed!");
+                    return EXIT_FAILURE;
+                }
+            }
+        }else{
+            pthread_mutex_unlock(&mutex_gnss);       /* up semaphore */
+        }
+
+        //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
+        if(snsDataReady)
+        {
+            snsDataReady=false;
+            LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT2);
+            LOG_DEBUG(gContext,"MsgID:%s", msgId);
+            LOG_DEBUG(gContext,"Len:%d", (int)strlen(sock_buf));
+            LOG_DEBUG(gContext,"Data:%s", sock_buf);
+
+            si_other.sin_port = htons(PORT2);
+            if(sendto(sock, sock_buf, strlen(sock_buf)+1, 0, (struct sockaddr *)&si_other, slen) == -1)
+            {
+                LOG_ERROR_MSG(gContext,"sendto() failed!");
+                return EXIT_FAILURE;
+            }
+        }
+        //VHL: list of supported message IDs
+        //char* vhlstr = "GVVEHVER,GVVEHENGSPEED,GVVEHFUELLEVEL,GVVEHFUELCONS,GVVEHTOTALODO";
+        if(vehDataReady)
+        {
+            vehDataReady=false;
+            LOG_DEBUG(gContext,"Sending Packet to %s:%d",ipaddr,PORT3);
+            LOG_DEBUG(gContext,"MsgID:%s", msgId);
+            LOG_DEBUG(gContext,"Len:%d", (int)strlen(sock_buf));
+            LOG_DEBUG(gContext,"Data:%s", sock_buf);
+
+            si_other.sin_port = htons(PORT3);
+            if(sendto(sock, sock_buf, strlen(sock_buf)+1, 0, (struct sockaddr *)&si_other, slen) == -1)
+            {
+                LOG_ERROR_MSG(gContext,"sendto() failed!");
+                return EXIT_FAILURE;
+            }
+        }
+
+        sleep(1);
+    } while(isRunning);
+
+    gnss_destroy();
+
+    /* restore the old port settings */
+    tcsetattr(g_obd2_fd,TCSANOW,&g_oldtio);
+
+    close(sock);
+
+    return EXIT_SUCCESS;
+}
+
+