some improvements to make it easier to maintain

10 files changed, 1721 insertions, 0 deletions

diff --git a/src/sensors-service/CMakeLists.txt b/src/sensors-service/CMakeLists.txt
new file mode 100644
index 0000000..129a509
--- /dev/null
+++ b/src/sensors-service/CMakeLists.txt
@@ -0,0 +1,77 @@
+###########################################################################
+# @licence app begin@
+# SPDX-License-Identifier: MPL-2.0
+#
+# Component Name: SensorsService
+#
+# Author: Marco Residori
+#
+# Copyright (C) 2013, XS Embedded GmbH
+# Copyright (C) 2018, PSA Groupe
+# 
+# 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/.
+#
+# Update (2014/12/02) : Philippe Colliot <philippe.colliot@mpsa.com>,
+#				PSA Peugeot Citroen
+#		- introduce debug flag to disable verbosity
+# Update (2018/05/30) : Philippe Colliot <philippe.colliot@mpsa.com>,
+#				PSA Groupe
+#		- adaptation of former version to introduce dead reckoning
+# @licence end@
+###########################################################################
+project(sensors-service)
+
+message(STATUS ${PROJECT_NAME})
+
+add_definitions("-std=gnu++11")
+
+find_package(PkgConfig REQUIRED)
+
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${TOP_DIR}/lib)
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${TOP_DIR}/bin)
+
+include_directories("${SENSORS_SERVICE_API_DIR}")
+
+set(LIBRARIES pthread)
+
+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()
+
+if(WITH_DEBUG)
+    add_definitions("-DDEBUG_ENABLED=1")
+endif()
+
+#generate library using replayer as input
+set(SRC ${CMAKE_CURRENT_SOURCE_DIR}/sns-use-replayer.c 
+     ${CMAKE_CURRENT_SOURCE_DIR}/wheeltick.c 
+     ${CMAKE_CURRENT_SOURCE_DIR}/gyroscope.c 
+     ${CMAKE_CURRENT_SOURCE_DIR}/acceleration.c
+     ${CMAKE_CURRENT_SOURCE_DIR}/vehicle-data.c
+     ${CMAKE_CURRENT_SOURCE_DIR}/vehicle-speed.c
+     ${CMAKE_CURRENT_SOURCE_DIR}/sns-meta-data.c)
+
+add_library(${PROJECT_NAME} SHARED ${SRC})
+target_link_libraries(${PROJECT_NAME} ${LIBRARIES})
+install(TARGETS ${PROJECT_NAME} DESTINATION lib)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/sensors-service/acceleration.c b/src/sensors-service/acceleration.c
new file mode 100644
index 0000000..bbb2bcc
--- /dev/null
+++ b/src/sensors-service/acceleration.c
@@ -0,0 +1,220 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \ingroup SensorsService
+* \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 "globals.h"
+#include "acceleration.h"
+#include "sns-meta-data.h"
+
+static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the callbacks
+static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
+
+static volatile AccelerationCallback cbAcceleration = 0;
+static TAccelerationData gAccelerationData = {0};
+TAccelerationConfiguration gAccelerationConfiguration;
+
+static TSensorStatus gStatus = {0};
+static volatile SensorStatusCallback cbStatus = 0;
+
+bool iAccelerationInit()
+{
+    pthread_mutex_lock(&mutexCb);
+    cbAcceleration = 0;
+    pthread_mutex_unlock(&mutexCb);
+    
+    pthread_mutex_lock(&mutexData);
+    gAccelerationData.validityBits = 0;
+    //example accelerometer configuration for a 3-axis accelerometer
+    gAccelerationConfiguration.dist2RefPointX = 0;
+    gAccelerationConfiguration.dist2RefPointY = 0;
+    gAccelerationConfiguration.dist2RefPointZ = 0;
+    gAccelerationConfiguration.angleYaw = 0;
+    gAccelerationConfiguration.anglePitch = 0;
+    gAccelerationConfiguration.angleRoll = 0;
+    gAccelerationConfiguration.sigmaX = 0;
+    gAccelerationConfiguration.sigmaY = 0;
+    gAccelerationConfiguration.sigmaZ = 0;
+    gAccelerationConfiguration.typeBits = 
+        ACCELERATION_X_PROVIDED |
+        ACCELERATION_Y_PROVIDED |
+        ACCELERATION_Z_PROVIDED;
+    gAccelerationConfiguration.validityBits = 
+      ACCELERATION_CONFIG_ANGLEYAW_VALID | 
+      ACCELERATION_CONFIG_ANGLEPITCH_VALID |
+      ACCELERATION_CONFIG_ANGLEROLL_VALID |
+      ACCELERATION_CONFIG_TYPE_VALID;
+    pthread_mutex_unlock(&mutexData);
+
+    return true;
+}
+
+bool iAccelerationDestroy()
+{
+    pthread_mutex_lock(&mutexCb);
+    cbAcceleration = 0;
+    pthread_mutex_unlock(&mutexCb);
+
+    return true;
+}
+
+bool snsAccelerationGetAccelerationData(TAccelerationData * accelerationData)
+{
+    bool retval = false;
+    if(accelerationData)
+    {
+        pthread_mutex_lock(&mutexData);
+        *accelerationData = gAccelerationData;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+    return retval;
+}
+
+bool snsAccelerationRegisterCallback(AccelerationCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    //only if valid callback and not already registered
+    if(callback && !cbAcceleration)
+    {
+        cbAcceleration = callback;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+bool snsAccelerationDeregisterCallback(AccelerationCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    if((cbAcceleration == callback) && callback)
+    {
+        cbAcceleration = 0;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+bool snsAccelerationGetMetaData(TSensorMetaData *data)
+{
+    bool retval = false;    
+    
+    if(data) 
+    {
+        pthread_mutex_lock(&mutexData);
+        *data = gSensorsMetaData[3];
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+
+    return retval;
+}
+
+bool snsAccelerationGetAccelerationConfiguration(TAccelerationConfiguration* config)
+{
+    bool retval = false; 
+    if(config) 
+    {
+        pthread_mutex_lock(&mutexData);
+        *config = gAccelerationConfiguration;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+
+    return retval;
+}
+
+void updateAccelerationData(const TAccelerationData accelerationData[], uint16_t numElements)
+{
+    if (accelerationData != NULL && numElements > 0)
+    {
+        pthread_mutex_lock(&mutexData);
+        gAccelerationData = accelerationData[numElements-1];
+        pthread_mutex_unlock(&mutexData);
+        pthread_mutex_lock(&mutexCb);
+        if (cbAcceleration)
+        {
+            cbAcceleration(accelerationData, numElements);
+        }
+        pthread_mutex_unlock(&mutexCb);
+    }
+}
+
+bool snsAccelerationGetStatus(TSensorStatus* status){
+    bool retval = false;
+    if(status)
+    {
+        pthread_mutex_lock(&mutexData);
+        *status = gStatus;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+    return retval;
+}
+
+bool snsAccelerationRegisterStatusCallback(SensorStatusCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    //only if valid callback and not already registered
+    if(callback && !cbStatus)
+    {
+        cbStatus = callback;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+bool snsAccelerationDeregisterStatusCallback(SensorStatusCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    if((cbStatus == callback) && callback)
+    {
+        cbStatus = 0;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+void updateAccelerationStatus(const TSensorStatus* status)
+{
+    if (status)
+    {
+        pthread_mutex_lock(&mutexData);
+        gStatus = *status;
+        pthread_mutex_unlock(&mutexData);
+        pthread_mutex_lock(&mutexCb);
+        if (cbStatus)
+        {
+            cbStatus(status);
+        }
+        pthread_mutex_unlock(&mutexCb);
+    }
+}
\ No newline at end of file
diff --git a/src/sensors-service/globals.h b/src/sensors-service/globals.h
new file mode 100644
index 0000000..7c2f9d0
--- /dev/null
+++ b/src/sensors-service/globals.h
@@ -0,0 +1,67 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \ingroup SensorsService
+* \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@
+**************************************************************************/
+
+#ifndef GLOBALS_H
+#define GLOBALS_H
+
+#include <stdbool.h>
+#include <pthread.h>
+#include <time.h>
+
+#include "sns-init.h"
+#include "wheel.h"
+#include "acceleration.h"
+#include "gyroscope.h"
+#include "vehicle-speed.h"
+#include "sns-meta-data.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern const TSensorMetaData gSensorsMetaData[];
+
+bool iAccelerationInit();
+bool iAccelerationDestroy();
+void updateAccelerationData(const TAccelerationData accelerationData[], uint16_t numElements);
+void updateAccelerationStatus(const TSensorStatus* status);
+
+bool iGyroscopeInit();
+bool iGyroscopeDestroy();
+void updateGyroscopeData(const TGyroscopeData gyroData[], uint16_t numElements);
+void updateGyroscopeStatus(const TSensorStatus* status);
+
+
+bool iWheelInit();
+bool iWheelDestroy();
+void updateWheelData(const TWheelData wheelData[], uint16_t numElements);
+void updateWheelStatus(const TSensorStatus* status);
+
+bool iVehicleSpeedInit();
+bool iVehicleSpeedDestroy();
+void updateVehicleSpeedData(const TVehicleSpeedData vehicleSpeedData[], uint16_t numElements);
+void updateVehicleSpeedStatus(const TSensorStatus* status);
+
+bool iVehicleDataInit();
+bool iVehicleDataDestroy();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* GLOBALS_H */
diff --git a/src/sensors-service/gyroscope.c b/src/sensors-service/gyroscope.c
new file mode 100644
index 0000000..a0bf7b6
--- /dev/null
+++ b/src/sensors-service/gyroscope.c
@@ -0,0 +1,217 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \ingroup SensorsService
+* \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 "globals.h"
+#include "gyroscope.h"
+#include "sns-meta-data.h"
+
+static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the callbacks
+static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
+
+static volatile GyroscopeCallback cbGyroscope = 0;
+static TGyroscopeData gGyroscopeData = {0};
+TGyroscopeConfiguration gGyroscopeConfiguration;
+
+static TSensorStatus gStatus = {0};
+static volatile SensorStatusCallback cbStatus = 0;
+
+
+bool iGyroscopeInit()
+{
+    pthread_mutex_lock(&mutexCb);
+    cbGyroscope = 0;
+    pthread_mutex_unlock(&mutexCb);
+
+    pthread_mutex_lock(&mutexData);
+    gGyroscopeData.validityBits = 0;
+    //example gyroscope configuration for a 3-axis gyro
+    gGyroscopeConfiguration.angleYaw = 0;
+    gGyroscopeConfiguration.anglePitch = 0;
+    gGyroscopeConfiguration.angleRoll = 0;
+    gGyroscopeConfiguration.momentOfYawInertia = 0;
+    gGyroscopeConfiguration.sigmaGyroscope = 0;
+    gGyroscopeConfiguration.typeBits = 
+        GYROSCOPE_YAWRATE_PROVIDED |
+        GYROSCOPE_PITCHRATE_PROVIDED |
+        GYROSCOPE_ROLLRATE_PROVIDED;
+    gGyroscopeConfiguration.validityBits = 
+      GYROSCOPE_CONFIG_ANGLEYAW_VALID | 
+      GYROSCOPE_CONFIG_ANGLEPITCH_VALID |
+      GYROSCOPE_CONFIG_ANGLEROLL_VALID |
+      GYROSCOPE_CONFIG_TYPE_VALID;
+    pthread_mutex_unlock(&mutexData);
+
+    return true;
+}
+
+bool iGyroscopeDestroy()
+{
+    pthread_mutex_lock(&mutexCb);
+    cbGyroscope = 0;
+    pthread_mutex_unlock(&mutexCb);
+
+    return true;
+}
+
+bool snsGyroscopeGetGyroscopeData(TGyroscopeData * gyroData)
+{
+    bool retval = false;
+    if(gyroData)
+    {
+        pthread_mutex_lock(&mutexData);
+        *gyroData = gGyroscopeData;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+    return retval;
+}
+
+bool snsGyroscopeRegisterCallback(GyroscopeCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    //only if valid callback and not already registered
+    if(callback && !cbGyroscope)
+    {
+        cbGyroscope = callback;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+bool snsGyroscopeDeregisterCallback(GyroscopeCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    if((cbGyroscope == callback) && callback)
+    {
+        cbGyroscope = 0;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+bool snsGyroscopeGetMetaData(TSensorMetaData *data)
+{
+    bool retval = false;    
+    
+    if(data) 
+    {
+        pthread_mutex_lock(&mutexData);
+        *data = gSensorsMetaData[1];
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+
+    return retval;
+}
+
+bool snsGyroscopeGetConfiguration(TGyroscopeConfiguration* gyroConfig)
+{
+    bool retval = false; 
+    if(gyroConfig) 
+    {
+        pthread_mutex_lock(&mutexData);
+        *gyroConfig = gGyroscopeConfiguration;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+
+    return retval;
+}
+
+void updateGyroscopeData(const TGyroscopeData gyroData[], uint16_t numElements)
+{
+    if (gyroData != NULL && numElements > 0)
+    {
+        pthread_mutex_lock(&mutexData);
+        gGyroscopeData = gyroData[numElements-1];
+        pthread_mutex_unlock(&mutexData);
+        pthread_mutex_lock(&mutexCb);
+        if (cbGyroscope)
+        {
+            cbGyroscope(gyroData, numElements);
+        }
+        pthread_mutex_unlock(&mutexCb);
+    }
+}
+
+bool snsGyroscopeGetStatus(TSensorStatus* status){
+    bool retval = false;
+    if(status)
+    {
+        pthread_mutex_lock(&mutexData);
+        *status = gStatus;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+    return retval;
+}
+
+bool snsGyroscopeRegisterStatusCallback(SensorStatusCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    //only if valid callback and not already registered
+    if(callback && !cbStatus)
+    {
+        cbStatus = callback;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+bool snsGyroscopeDeregisterStatusCallback(SensorStatusCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    if((cbStatus == callback) && callback)
+    {
+        cbStatus = 0;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+void updateGyroscopeStatus(const TSensorStatus* status)
+{
+    if (status)
+    {
+        pthread_mutex_lock(&mutexData);
+        gStatus = *status;
+        pthread_mutex_unlock(&mutexData);
+        pthread_mutex_lock(&mutexCb);
+        if (cbStatus)
+        {
+            cbStatus(status);
+        }
+        pthread_mutex_unlock(&mutexCb);
+    }
+}
\ No newline at end of file
diff --git a/src/sensors-service/log.h b/src/sensors-service/log.h
new file mode 100644
index 0000000..fa5c3ca
--- /dev/null
+++ b/src/sensors-service/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/sensors-service/sns-meta-data.c b/src/sensors-service/sns-meta-data.c
new file mode 100644
index 0000000..2b218d1
--- /dev/null
+++ b/src/sensors-service/sns-meta-data.c
@@ -0,0 +1,48 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \ingroup SensorsService
+* \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 "globals.h"
+#include "sns-meta-data.h"
+
+#define NUM_SENSORS 4
+
+const TSensorMetaData gSensorsMetaData[NUM_SENSORS] = {
+    {GENIVI_SNS_API_MAJOR,        //version
+     SENSOR_CATEGORY_PHYSICAL,    //category
+     SENSOR_TYPE_WHEELTICK,       //type
+     100},                        //cycleTime in ms
+    {GENIVI_SNS_API_MAJOR,        //version
+     SENSOR_CATEGORY_PHYSICAL,    //category
+     SENSOR_TYPE_GYROSCOPE,       //type
+     100},                        //cycleTime in ms
+    {GENIVI_SNS_API_MAJOR,        //version
+     SENSOR_CATEGORY_PHYSICAL,    //category
+     SENSOR_TYPE_VEHICLE_SPEED,   //type
+     100},                         //cycleTime in ms
+    {GENIVI_SNS_API_MAJOR,        //version
+     SENSOR_CATEGORY_PHYSICAL,    //category
+     SENSOR_TYPE_ACCELERATION,    //type
+     100}                         //cycleTime in ms     
+};
+
+int32_t getSensorMetaDataList(const TSensorMetaData** metadata)
+{
+    *metadata = gSensorsMetaData;
+
+    return NUM_SENSORS;
+}
diff --git a/src/sensors-service/sns-use-replayer.c b/src/sensors-service/sns-use-replayer.c
new file mode 100644
index 0000000..a628453
--- /dev/null
+++ b/src/sensors-service/sns-use-replayer.c
@@ -0,0 +1,517 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \ingroup SensorsService
+* \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 "sns-init.h"
+#include "acceleration.h"
+#include "gyroscope.h"
+#include "inclination.h"
+#include "odometer.h"
+#include "reverse-gear.h"
+#include "slip-angle.h"
+#include "steering-angle.h"
+#include "vehicle-data.h"
+#include "vehicle-speed.h"
+#include "vehicle-state.h"
+#include "wheel.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <time.h>
+#include <errno.h>
+#include <pthread.h>
+#include <assert.h>
+
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <memory.h>
+
+#include "globals.h"
+#include "log.h"
+
+#define STRINGIFY2( x) #x
+#define STRINGIFY(x) STRINGIFY2(x)
+
+#define BUFLEN 256
+#define MSGIDLEN 20
+#define PORT 9931
+
+#define MAX_BUF_MSG 16
+
+DLT_DECLARE_CONTEXT(gContext);
+
+//Listener thread
+static pthread_t listenerThread;
+//Listener thread loop control variale
+static volatile bool isRunning = false;
+//Socket file descriptor used by listener thread. 
+//Global so we can shutdown() it to release listener thread immediately from waiting 
+static int s = 0;
+//Note: we do not mutex-protect the above globals because for this proof-of-concept 
+//implementation we expect that the client does not ake overlapping calls.
+//For a real-world fool-proof implementation you would have to add more checks.
+
+
+static void *listenForMessages( void *ptr );
+
+bool snsInit()
+{
+    isRunning = true;
+
+    if(pthread_create( &listenerThread, NULL, listenForMessages, NULL) != 0)
+    {
+        isRunning = false;
+        return false;
+    }
+
+    return true;
+}
+
+bool snsDestroy()
+{
+    isRunning = false;
+
+    //shut down the socket
+    shutdown(s,2);
+
+    if(listenerThread)
+    {
+        pthread_join( listenerThread, NULL);
+    }
+
+    return true;
+}
+
+void snsGetVersion(int *major, int *minor, int *micro)
+{
+    if(major)
+    {
+        *major = GENIVI_SNS_API_MAJOR;
+    }
+
+    if(minor)
+    {
+        *minor = GENIVI_SNS_API_MINOR;
+    }
+
+    if(micro)
+    {
+        *micro = GENIVI_SNS_API_MICRO;
+    }
+}
+
+bool snsAccelerationInit()
+{
+    return iAccelerationInit();
+}
+
+bool snsAccelerationDestroy()
+{
+    return iAccelerationDestroy();
+}
+
+bool snsGyroscopeInit()
+{
+    return iGyroscopeInit();
+}
+
+bool snsGyroscopeDestroy()
+{
+    return iGyroscopeDestroy();
+}
+
+bool snsVehicleSpeedInit()
+{
+    return iVehicleSpeedInit();
+}
+
+bool snsVehicleSpeedDestroy()
+{
+    return iVehicleSpeedDestroy();
+}
+
+bool snsWheelInit()
+{
+    return iWheelInit();
+}
+
+bool snsWheelDestroy()
+{
+    return iWheelDestroy();
+}
+
+bool snsVehicleDataInit()
+{
+    return iVehicleDataInit();
+}
+
+bool snsVehicleDataDestroy()
+{
+    return iVehicleDataDestroy();
+}
+
+bool processGVSNSWHE(const char* data)
+{
+    //parse data like: 15259,0,$GVSNSWHE,15200,35,45,0,0,0,0,0,0,0X0001,100,0X03
+    
+    //storage for buffered data
+    static TWheelData buf_whtk[MAX_BUF_MSG];
+    static uint16_t buf_size = 0;
+    static uint16_t last_countdown = 0;    
+
+    uint64_t timestamp;
+    uint16_t countdown;
+    TWheelData whtk = { 0 };
+    int n = 0;
+
+    if(!data)
+    {
+        LOG_ERROR_MSG(gContext,"wrong parameter!");
+        return false;
+    }
+
+    //First try to read in new format with measurementInterval
+    n = sscanf(data, "%llu,%hu,$GVSNSWHE,%llu,%f,%f,%f,%f,%f,%f,%f,%f,%x,%u,%x", 
+        &timestamp, &countdown, &whtk.timestamp
+        ,&whtk.data[0], &whtk.data[1]
+        ,&whtk.data[2], &whtk.data[3]
+        ,&whtk.data[4], &whtk.data[5]
+        ,&whtk.data[6], &whtk.data[7]
+        ,&whtk.statusBits
+        ,&whtk.measurementInterval
+        ,&whtk.validityBits
+        );
+
+    if (n != 14) //14 fields to parse
+    {
+        //Else try to read in old format without measurementInterval
+        n = sscanf(data, "%llu,%hu,$GVSNSWHE,%llu,%f,%f,%f,%f,%f,%f,%f,%f,%x,%x", 
+            &timestamp, &countdown, &whtk.timestamp
+            ,&whtk.data[0], &whtk.data[1]
+            ,&whtk.data[2], &whtk.data[3]
+            ,&whtk.data[4], &whtk.data[5]
+            ,&whtk.data[6], &whtk.data[7]
+            ,&whtk.statusBits
+            ,&whtk.validityBits
+            );
+
+        if (n != 13) //13 fields to parse
+        {
+            LOG_ERROR_MSG(gContext,"replayer: processGVSNSWHE failed!");
+            return false;
+        }
+    }
+
+    //buffered data handling
+    if (countdown < MAX_BUF_MSG) //enough space in buffer?
+    {
+        if (buf_size == 0) //a new sequence starts
+        {
+            buf_size = countdown+1;
+            last_countdown = countdown;
+            buf_whtk[buf_size-countdown-1] = whtk;
+        }
+        else if ((last_countdown-countdown) == 1) //sequence continued
+        {
+            last_countdown = countdown;
+            buf_whtk[buf_size-countdown-1] = whtk;
+        }
+        else //sequence interrupted: clear buffer
+        {
+            buf_size = 0;
+            last_countdown = 0;
+        }
+    }
+    else //clear buffer
+    {
+        buf_size = 0;
+        last_countdown = 0;
+    }
+
+    if((countdown == 0) && (buf_size >0) )
+    {
+        updateWheelData(buf_whtk, buf_size);
+        buf_size = 0;
+        last_countdown = 0;        
+    }
+
+    return true;
+}
+
+static bool processGVSNSGYR(const char* data)
+{
+    //parse data like: 061074000,0$GVSNSGYR,061074000,-38.75,0,0,0,0X01
+    
+    //storage for buffered data
+    static TGyroscopeData buf_gyro[MAX_BUF_MSG];
+    static uint16_t buf_size = 0;
+    static uint16_t last_countdown = 0;    
+
+    uint64_t timestamp;
+    uint16_t countdown;
+    TGyroscopeData gyro = { 0 };
+    int n = 0;
+
+    if(!data )
+    {
+        LOG_ERROR_MSG(gContext,"wrong parameter!");
+        return false;
+    }
+
+    //First try to read in new format with measurementInterval
+    n = sscanf(data, "%llu,%hu,$GVSNSGYR,%llu,%f,%f,%f,%f,%u,%x"
+        ,&timestamp, &countdown, &gyro.timestamp
+        ,&gyro.yawRate
+        ,&gyro.pitchRate
+        ,&gyro.rollRate
+        ,&gyro.temperature
+        ,&gyro.measurementInterval        
+        ,&gyro.validityBits
+        );
+    if (n != 9) //9 fields to parse
+    {
+        //Else try to read in old format without measurementInterval
+        n = sscanf(data, "%llu,%hu,$GVSNSGYR,%llu,%f,%f,%f,%f,%x"
+            ,&timestamp, &countdown, &gyro.timestamp
+            ,&gyro.yawRate
+            ,&gyro.pitchRate
+            ,&gyro.rollRate
+            ,&gyro.temperature
+            ,&gyro.validityBits
+            );
+        
+        if (n != 8) //8 fields to parse
+        {
+                LOG_ERROR_MSG(gContext,"replayer: processGVSNSGYR failed!");
+                return false;
+        }
+    }
+
+    //buffered data handling
+    if (countdown < MAX_BUF_MSG) //enough space in buffer?
+    {
+        if (buf_size == 0) //a new sequence starts
+        {
+            buf_size = countdown+1;
+            last_countdown = countdown;
+            buf_gyro[buf_size-countdown-1] = gyro;
+        }
+        else if ((last_countdown-countdown) == 1) //sequence continued
+        {
+            last_countdown = countdown;
+            buf_gyro[buf_size-countdown-1] = gyro;
+        }
+        else //sequence interrupted: clear buffer
+        {
+            buf_size = 0;
+            last_countdown = 0;
+        }
+    }
+    else //clear buffer
+    {
+        buf_size = 0;
+        last_countdown = 0;
+    }
+
+    if((countdown == 0) && (buf_size >0) )
+    {
+        updateGyroscopeData(buf_gyro,buf_size);
+        buf_size = 0;
+        last_countdown = 0;        
+    }
+
+    return true;
+}
+
+
+
+static bool processGVSNSVSP(const char* data)
+{
+    //parse data like: 061074000,0$GVSNSVSP,061074000,0.51,0X01
+    
+    //storage for buffered data
+    static TVehicleSpeedData buf_vehsp[MAX_BUF_MSG];
+    static uint16_t buf_size = 0;
+    static uint16_t last_countdown = 0;    
+
+    uint64_t timestamp;
+    uint16_t countdown;
+    TVehicleSpeedData vehsp = { 0 };
+    int n = 0;
+
+    if(!data)
+    {
+        LOG_ERROR_MSG(gContext,"wrong parameter!");
+        return false;
+    }
+
+    //First try to read in new format with measurementInterval
+    n = sscanf(data, "%llu,%hu,$GVSNSVSP,%llu,%f,%u,%x"
+        ,&timestamp
+        ,&countdown
+        ,&vehsp.timestamp
+        ,&vehsp.vehicleSpeed
+        ,&vehsp.measurementInterval
+        ,&vehsp.validityBits
+        );    
+
+    if (n != 6) //6 fields to parse
+    {
+        //Else try to read in old format without measurementInterval
+    n = sscanf(data, "%llu,%hu,$GVSNSVSP,%llu,%f,%x"
+        ,&timestamp
+        ,&countdown
+        ,&vehsp.timestamp
+        ,&vehsp.vehicleSpeed
+        ,&vehsp.validityBits
+        );    
+
+        if (n != 5) //5 fields to parse
+        {
+            LOG_ERROR_MSG(gContext,"replayer: processGVSNSVSP failed!");
+            return false;
+        }
+    }
+
+    //buffered data handling
+    if (countdown < MAX_BUF_MSG) //enough space in buffer?
+    {
+        if (buf_size == 0) //a new sequence starts
+        {
+            buf_size = countdown+1;
+            last_countdown = countdown;
+            buf_vehsp[buf_size-countdown-1] = vehsp;
+        }
+        else if ((last_countdown-countdown) == 1) //sequence continued
+        {
+            last_countdown = countdown;
+            buf_vehsp[buf_size-countdown-1] = vehsp;
+        }
+        else //sequence interrupted: clear buffer
+        {
+            buf_size = 0;
+            last_countdown = 0;
+        }
+    }
+    else //clear buffer
+    {
+        buf_size = 0;
+        last_countdown = 0;
+    }
+
+    if((countdown == 0) && (buf_size >0) )
+    {
+        updateVehicleSpeedData(buf_vehsp,buf_size);
+        buf_size = 0;
+        last_countdown = 0;        
+    }
+
+    return true;
+}
+
+static void *listenForMessages( void *ptr )
+{  
+    struct sockaddr_in si_me;
+    struct sockaddr_in si_other;
+    socklen_t slen = sizeof(si_other);
+    ssize_t readBytes = 0;
+    char buf[BUFLEN+1]; //add space fer terminating \0
+    char msgId[MSGIDLEN+1]; //add space fer terminating \0
+    int port = PORT;
+
+    DLT_REGISTER_APP("SNSS", "SENSOSRS-SERVICE");
+    DLT_REGISTER_CONTEXT(gContext,"SSRV", "Global Context");
+
+    LOG_INFO(gContext,"SensorsService listening on port %d...",port);
+
+    if((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1)
+    {
+        LOG_ERROR_MSG(gContext,"socket() failed!");
+        exit(EXIT_FAILURE);
+    }
+
+    memset((char *) &si_me, 0, sizeof(si_me));
+    si_me.sin_family = AF_INET;
+
+    si_me.sin_port = htons(port);
+    si_me.sin_addr.s_addr = htonl(INADDR_ANY);
+    if(bind(s, (struct sockaddr *)&si_me, sizeof(si_me)) == -1)
+    {
+        LOG_ERROR_MSG(gContext,"socket() failed!");
+        exit(EXIT_FAILURE);
+    }
+
+    while(isRunning == true)
+    {
+        //use select to introduce a timeout - alloy shutdown even when no data are received
+        fd_set readfs;    /* file descriptor set */
+        int    maxfd;     /* maximum file desciptor used */        
+        int res;
+        struct timeval Timeout;
+        /* set timeout value within input loop */
+        Timeout.tv_usec = 0;  /* milliseconds */
+        Timeout.tv_sec  = 1;  /* seconds */
+        FD_SET(s, &readfs);
+        maxfd = s+1;
+        /* block until input becomes available */
+        res = select(maxfd, &readfs, NULL, NULL, &Timeout);
+
+        if (res > 0)
+        {
+            
+            readBytes = recvfrom(s, buf, BUFLEN, 0, (struct sockaddr *)&si_other, &slen);
+
+            if(readBytes < 0)
+            {
+                LOG_ERROR_MSG(gContext,"recvfrom() failed!");
+                exit(EXIT_FAILURE);
+            }
+            
+            buf[readBytes] = '\0';
+
+            sscanf(buf, "%*[^'$']$%" STRINGIFY(MSGIDLEN) "[^',']", msgId);
+    
+            LOG_DEBUG(gContext,"Data:%s", buf);
+
+            if(strcmp("GVSNSGYR", msgId) == 0)
+            {
+                processGVSNSGYR(buf);
+            }
+            else if(strcmp("GVSNSWHE", msgId) == 0)
+            {
+                processGVSNSWHE(buf);
+            }
+            else if(strcmp("GVSNSVSP", msgId) == 0)
+            {
+                processGVSNSVSP(buf);
+            }
+        }
+    }
+
+    close(s);
+
+    return EXIT_SUCCESS;
+}
+
+
+
+
diff --git a/src/sensors-service/vehicle-data.c b/src/sensors-service/vehicle-data.c
new file mode 100644
index 0000000..fc6ea2b
--- /dev/null
+++ b/src/sensors-service/vehicle-data.c
@@ -0,0 +1,68 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \ingroup SensorsService
+* \author Helmut Schmidt <Helmut.3.Schmidt@continental-corporation.com>
+*
+* \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 "globals.h"
+#include "vehicle-data.h"
+#include "sns-meta-data.h"
+
+static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the callbacks
+static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
+
+TVehicleDataConfiguration gVehicleDataConfiguration = {0};
+
+
+bool iVehicleDataInit()
+{
+
+    pthread_mutex_lock(&mutexData);
+    //example vehicle data configuration
+    gVehicleDataConfiguration.vehicleType = SNS_CAR;
+    gVehicleDataConfiguration.drivenAxles = SNS_AXLE_FRONT;    
+    gVehicleDataConfiguration.trackWidth = 1.52;
+    gVehicleDataConfiguration.wheelBase = 2.31;
+    gVehicleDataConfiguration.validityBits = 
+      VEHICLESTATUS_VEHICLETYPE_VALID | 
+      VEHICLESTATUS_DRIVENAXLES_VALID |
+      VEHICLESTATUS_TRACKWIDTH_VALID |
+      VEHICLESTATUS_WHELLBASE_VALID;
+    pthread_mutex_unlock(&mutexData);
+
+    return true;
+}
+
+bool iVehicleDataDestroy()
+{
+    return true;
+}
+
+
+bool snsVehicleDataGetConfiguration(TVehicleDataConfiguration* vehicleDataConfiguration)
+{
+    bool retval = false; 
+    
+    if(vehicleDataConfiguration) 
+    {
+        pthread_mutex_lock(&mutexData);
+        *vehicleDataConfiguration = gVehicleDataConfiguration;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+
+    return retval;
+}
+
diff --git a/src/sensors-service/vehicle-speed.c b/src/sensors-service/vehicle-speed.c
new file mode 100644
index 0000000..994d69c
--- /dev/null
+++ b/src/sensors-service/vehicle-speed.c
@@ -0,0 +1,181 @@
+/**************************************************************************
+* Component Name: SensorsService
+* Author: Marco Residori <marco.residori@xse.de>
+*
+* 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/.
+*
+**************************************************************************/
+
+#include "globals.h"
+#include "vehicle-speed.h"
+#include "sns-meta-data.h"
+
+static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the callbacks
+static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
+
+static volatile VehicleSpeedCallback cbVehicleSpeed = 0;
+static TVehicleSpeedData gVehicleSpeedData = {0};
+
+static TSensorStatus gStatus = {0};
+static volatile SensorStatusCallback cbStatus = 0;
+
+bool iVehicleSpeedInit()
+{
+    pthread_mutex_lock(&mutexCb);
+    cbVehicleSpeed = 0;
+    pthread_mutex_unlock(&mutexCb);
+
+    pthread_mutex_lock(&mutexData);
+    gVehicleSpeedData.validityBits = 0;
+    pthread_mutex_unlock(&mutexData);
+
+    return true;
+}
+
+bool iVehicleSpeedDestroy()
+{
+    pthread_mutex_lock(&mutexCb);
+    cbVehicleSpeed = 0;
+    pthread_mutex_unlock(&mutexCb);
+
+    return true;
+}
+
+bool snsVehicleSpeedGetVehicleSpeedData(TVehicleSpeedData* vehicleSpeed)
+{
+    bool retval = false;
+    if(vehicleSpeed)
+    {
+        pthread_mutex_lock(&mutexData);
+        *vehicleSpeed = gVehicleSpeedData;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+    return retval;
+}
+
+bool snsVehicleSpeedRegisterCallback(VehicleSpeedCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    //only if valid callback and not already registered
+    if(callback && !cbVehicleSpeed)
+    {
+        cbVehicleSpeed = callback;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+bool snsVehicleSpeedDeregisterCallback(VehicleSpeedCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    if((cbVehicleSpeed == callback) && callback)
+    {
+        cbVehicleSpeed = 0;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+bool snsVehicleSpeedGetMetaData(TSensorMetaData *data)
+{
+    bool retval = false;    
+    
+    if(data) 
+    {
+        pthread_mutex_lock(&mutexData);
+        *data = gSensorsMetaData[2];
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+
+    return retval;
+}
+
+void updateVehicleSpeedData(const TVehicleSpeedData vehicleSpeedData[], uint16_t numElements)
+{
+    if (vehicleSpeedData != NULL && numElements > 0)
+    {
+        pthread_mutex_lock(&mutexData);
+        gVehicleSpeedData = vehicleSpeedData[numElements-1];
+        pthread_mutex_unlock(&mutexData);
+        pthread_mutex_lock(&mutexCb);
+        if (cbVehicleSpeed)
+        {
+            cbVehicleSpeed(vehicleSpeedData, numElements);
+        }
+        pthread_mutex_unlock(&mutexCb);
+    }
+}
+
+bool snsVehicleSpeedGetStatus(TSensorStatus* status){
+    bool retval = false;
+    if(status)
+    {
+        pthread_mutex_lock(&mutexData);
+        *status = gStatus;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+    return retval;
+}
+
+bool snsVehicleSpeedRegisterStatusCallback(SensorStatusCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    //only if valid callback and not already registered
+    if(callback && !cbStatus)
+    {
+        cbStatus = callback;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+bool snsVehicleSpeedDeregisterStatusCallback(SensorStatusCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    if((cbStatus == callback) && callback)
+    {
+        cbStatus = 0;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+void updateVehicleSpeedStatus(const TSensorStatus* status)
+{
+    if (status)
+    {
+        pthread_mutex_lock(&mutexData);
+        gStatus = *status;
+        pthread_mutex_unlock(&mutexData);
+        pthread_mutex_lock(&mutexCb);
+        if (cbStatus)
+        {
+            cbStatus(status);
+        }
+        pthread_mutex_unlock(&mutexCb);
+    }
+}
\ No newline at end of file
diff --git a/src/sensors-service/wheeltick.c b/src/sensors-service/wheeltick.c
new file mode 100644
index 0000000..d582d92
--- /dev/null
+++ b/src/sensors-service/wheeltick.c
@@ -0,0 +1,172 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \ingroup SensorsService
+* \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 "globals.h"
+#include "wheel.h"
+
+static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the callbacks
+static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
+
+static volatile WheelCallback cbWheel = 0;
+static TWheelData gWheelData = {0};
+
+static TSensorStatus gStatus = {0};
+static volatile SensorStatusCallback cbStatus = 0;
+
+bool iWheelInit()
+{
+    int i;
+
+    pthread_mutex_lock(&mutexCb);
+    cbWheel = 0;
+    pthread_mutex_unlock(&mutexCb);
+
+    pthread_mutex_lock(&mutexData);
+    gWheelData.validityBits = 0;
+    pthread_mutex_unlock(&mutexData);
+
+    return true;
+}
+
+bool iWheelDestroy()
+{
+    pthread_mutex_lock(&mutexCb);
+    cbWheel = 0;
+    pthread_mutex_unlock(&mutexCb);
+
+    return true;
+}
+
+bool snsWheelGetWheelData(TWheelData *wheelData)
+{
+    bool retval = false;
+    if(wheelData)
+    {
+        pthread_mutex_lock(&mutexData);
+        *wheelData = gWheelData;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+    return retval;
+}
+
+bool snsWheelRegisterCallback(WheelCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    //only if valid callback and not already registered
+    if(callback && !cbWheel)
+    {
+        cbWheel = callback;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+bool snsWheelDeregisterCallback(WheelCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    if((cbWheel == callback) && callback)
+    {
+        cbWheel = 0;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+void updateWheelData(const TWheelData wheelData[], uint16_t numElements)
+{
+    if (wheelData != NULL && numElements > 0)
+    {
+        pthread_mutex_lock(&mutexData);
+        gWheelData = wheelData[numElements-1];
+        pthread_mutex_unlock(&mutexData);
+        pthread_mutex_lock(&mutexCb);
+        if (cbWheel)
+        {
+            cbWheel(wheelData, numElements);
+        }
+        pthread_mutex_unlock(&mutexCb);
+    }
+}
+
+bool snsWheelGetStatus(TSensorStatus* status){
+    bool retval = false;
+    if(status)
+    {
+        pthread_mutex_lock(&mutexData);
+        *status = gStatus;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
+    }
+    return retval;
+}
+
+bool snsWheelRegisterStatusCallback(SensorStatusCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    //only if valid callback and not already registered
+    if(callback && !cbStatus)
+    {
+        cbStatus = callback;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+bool snsWheelDeregisterStatusCallback(SensorStatusCallback callback)
+{
+    bool retval = false;
+
+    pthread_mutex_lock(&mutexCb);
+    if((cbStatus == callback) && callback)
+    {
+        cbStatus = 0;
+        retval = true;
+    }
+    pthread_mutex_unlock(&mutexCb);
+
+    return retval;
+}
+
+void updateWheelStatus(const TSensorStatus* status)
+{
+    if (status)
+    {
+        pthread_mutex_lock(&mutexData);
+        gStatus = *status;
+        pthread_mutex_unlock(&mutexData);
+        pthread_mutex_lock(&mutexCb);
+        if (cbStatus)
+        {
+            cbStatus(status);
+        }
+        pthread_mutex_unlock(&mutexCb);
+    }
+}
\ No newline at end of file