Positioning PoC: rework register functions, improve NMEA parser, add logger, add support for real gyro/accelerometer sensor

13 files changed, 1400 insertions, 249 deletions

diff --git a/sensors-service/CMakeLists.txt b/sensors-service/CMakeLists.txt
index e251637..ec6faf0 100644
--- a/sensors-service/CMakeLists.txt
+++ b/sensors-service/CMakeLists.txt
@@ -34,6 +34,9 @@ option(WITH_REPLAYER
 option(WITH_IPHONE
     "Use IPHONE as source of sensors data" OFF)
 
+option(WITH_MPU6050
+    "Use MPU6050 as source of gyro/acceleration data" OFF)
+
 option(WITH_TESTS
     "Compile test applications" OFF)
 
diff --git a/sensors-service/src/CMakeLists.txt b/sensors-service/src/CMakeLists.txt
index 6df3397..74f2d0d 100644
--- a/sensors-service/src/CMakeLists.txt
+++ b/sensors-service/src/CMakeLists.txt
@@ -23,6 +23,7 @@ message(STATUS "LIB-SENSORS-SERVICE")
 message(STATUS "WITH_DLT = ${WITH_DLT}")
 message(STATUS "WITH_IPHONE = ${WITH_IPHONE}")
 message(STATUS "WITH_REPLAYER = ${WITH_REPLAYER}")
+message(STATUS "WITH_MPU6050 = ${WITH_MPU6050}")
 message(STATUS "WITH_TESTS = ${WITH_TESTS}")
 message(STATUS "WITH_DEBUG = ${WITH_DEBUG}")
 
@@ -55,6 +56,21 @@ if(WITH_IPHONE)
     add_library(sensors-service-use-iphone SHARED ${LIB_SRC_USE_IPHONE})
     target_link_libraries(sensors-service-use-iphone ${LIBRARIES})
     install(TARGETS sensors-service-use-iphone DESTINATION lib)
+elseif(WITH_MPU6050)
+    #generate library using replayer as input
+    set(LIB_SRC_USE_MPU6050 ${CMAKE_CURRENT_SOURCE_DIR}/sns-use-mpu6050.cpp
+             ${CMAKE_CURRENT_SOURCE_DIR}/mpu6050.cpp
+             ${CMAKE_CURRENT_SOURCE_DIR}/gyroscope.c
+             ${CMAKE_CURRENT_SOURCE_DIR}/acceleration.c
+             ${CMAKE_CURRENT_SOURCE_DIR}/vehicle-speed.c
+             ${CMAKE_CURRENT_SOURCE_DIR}/wheeltick.c
+             ${CMAKE_CURRENT_SOURCE_DIR}/sns-meta-data.c)
+    add_library(sensors-service-use-mpu6050 SHARED ${LIB_SRC_USE_MPU6050})
+    target_link_libraries(sensors-service-use-mpu6050 ${LIBRARIES})
+    install(TARGETS sensors-service-use-mpu6050 DESTINATION lib)
+    #for glibc <2.17, clock_gettime is in librt: http://linux.die.net/man/2/clo$
+    #TODO: is there a nice way to detect glibc version in CMake?
+    set(LIBRARIES ${LIBRARIES} rt)
 elseif(WITH_REPLAYER)
     #generate library using replayer as input
     set(LIB_SRC_USE_REPLAYER ${CMAKE_CURRENT_SOURCE_DIR}/sns-use-replayer.c 
diff --git a/sensors-service/src/acceleration.c b/sensors-service/src/acceleration.c
index 6e501e0..1ecc7b2 100644
--- a/sensors-service/src/acceleration.c
+++ b/sensors-service/src/acceleration.c
@@ -20,11 +20,14 @@
 #include "acceleration.h"
 #include "sns-meta-data.h"
 
-AccelerationCallback cbAcceleration = 0;
-TAccelerationData gAccelerationData;
+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;
 TAccelerationConfiguration gAccelerationConfiguration;
 
-bool snsAccelerationInit()
+bool iAccelerationInit()
 {
     pthread_mutex_lock(&mutexCb);
     cbAcceleration = 0;
@@ -55,7 +58,7 @@ bool snsAccelerationInit()
     return true;
 }
 
-bool snsAccelerationDestroy()
+bool iAccelerationDestroy()
 {
     pthread_mutex_lock(&mutexCb);
     cbAcceleration = 0;
@@ -66,85 +69,90 @@ bool snsAccelerationDestroy()
 
 bool snsAccelerationGetAccelerationData(TAccelerationData * accelerationData)
 {
-    if(!accelerationData)
+    bool retval = false;
+    if(accelerationData)
     {
-    	return false;
+        pthread_mutex_lock(&mutexData);
+        *accelerationData = gAccelerationData;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
     }
-
-    pthread_mutex_lock(&mutexData);
-    *accelerationData = gAccelerationData;
-    pthread_mutex_unlock(&mutexData);
-
-    return true;
+    return retval;
 }
 
 bool snsAccelerationRegisterCallback(AccelerationCallback callback)
 {
-    if(!callback)
-    {
-        return false;
-    }
+    bool retval = false;
 
-    //printf("snsAccelerationRegisterCallback\n");
-    pthread_mutex_lock(&mutexCb);
-    if(cbAcceleration != 0) 
-    {   
-        //already registered
+    //only if valid callback and not already registered
+    if(callback && !cbAcceleration)
+    {
+        pthread_mutex_lock(&mutexCb);
+        cbAcceleration = callback;
         pthread_mutex_unlock(&mutexCb);
-        return false;
+        retval = true;
     }
-    cbAcceleration = callback;
-    pthread_mutex_unlock(&mutexCb);
-
-    return true;
+    return retval;
 }
 
 bool snsAccelerationDeregisterCallback(AccelerationCallback callback)
 {
-    if(!callback)
-    {
-        return false;
-    }
+    bool retval = false;
 
-    //printf("snsAccelerationDeregisterCallback\n");
-    pthread_mutex_lock(&mutexCb);
-    if(cbAcceleration == callback)
+    if((cbAcceleration == callback) && callback)
     {
+        pthread_mutex_lock(&mutexCb);
         cbAcceleration = 0;
+        pthread_mutex_unlock(&mutexCb);
+        retval = true;
     }
-    pthread_mutex_unlock(&mutexCb);
 
-    return true;
+    return retval;
 }
 
 bool snsAccelerationGetMetaData(TSensorMetaData *data)
 {
-    if(!data) 
+    bool retval = false;    
+    
+    if(data) 
     {
-        return false;
+        pthread_mutex_lock(&mutexData);
+        *data = gSensorsMetaData[3];
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
     }
-    
-    pthread_mutex_lock(&mutexData);
-    *data = gSensorsMetaData[3];
-    pthread_mutex_unlock(&mutexData);
 
-    return true;
+    return retval;
 }
 
 bool snsAccelerationGetAccelerationConfiguration(TAccelerationConfiguration* config)
 {
-    if(!config) 
+    bool retval = false; 
+    if(config) 
     {
-        return false;
+        pthread_mutex_lock(&mutexData);
+        *config = gAccelerationConfiguration;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
     }
 
-    pthread_mutex_lock(&mutexData);
-    *config = gAccelerationConfiguration;
-    pthread_mutex_unlock(&mutexData);
-
-    return 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);
+    }
+}
 
 
diff --git a/sensors-service/src/globals.h b/sensors-service/src/globals.h
index c9280f7..bbda374 100644
--- a/sensors-service/src/globals.h
+++ b/sensors-service/src/globals.h
@@ -25,20 +25,36 @@
 
 #include "sns-init.h"
 #include "wheel.h"
+#include "acceleration.h"
 #include "gyroscope.h"
 #include "vehicle-speed.h"
 #include "sns-meta-data.h"
 
-extern pthread_mutex_t mutexCb;
-extern pthread_mutex_t mutexData;
+#ifdef __cplusplus
+extern "C" {
+#endif
 
-extern TGyroscopeData gGyroscopeData;
-extern TWheelticks gWheelticks;
-TVehicleSpeedData gVehicleSpeedData;
 extern const TSensorMetaData gSensorsMetaData[];
 
-extern WheeltickCallback cbWheelticks;
-extern GyroscopeCallback cbGyroscope;
-VehicleSpeedCallback cbVehicleSpeed;
+bool iAccelerationInit();
+bool iAccelerationDestroy();
+void updateAccelerationData(const TAccelerationData accelerationData[], uint16_t numElements);
+
+bool iGyroscopeInit();
+bool iGyroscopeDestroy();
+void updateGyroscopeData(const TGyroscopeData gyroData[], uint16_t numElements);
+
+
+bool iWheeltickInit();
+bool iWheeltickDestroy();
+void updateWheelticks(const TWheelticks ticks[], uint16_t numElements);
+
+bool iVehicleSpeedInit();
+bool iVehicleSpeedDestroy();
+void updateVehicleSpeedData(const TVehicleSpeedData vehicleSpeedData[], uint16_t numElements);
+
+#ifdef __cplusplus
+}
+#endif
 
 #endif /* GLOBALS_H */
diff --git a/sensors-service/src/gyroscope.c b/sensors-service/src/gyroscope.c
index 002edb3..fa1b2b1 100644
--- a/sensors-service/src/gyroscope.c
+++ b/sensors-service/src/gyroscope.c
@@ -20,11 +20,14 @@
 #include "gyroscope.h"
 #include "sns-meta-data.h"
 
-GyroscopeCallback cbGyroscope = 0;
-TGyroscopeData gGyroscopeData;
+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;
 TGyroscopeConfiguration gGyroscopeConfiguration;
 
-bool snsGyroscopeInit()
+bool iGyroscopeInit()
 {
     pthread_mutex_lock(&mutexCb);
     cbGyroscope = 0;
@@ -51,7 +54,7 @@ bool snsGyroscopeInit()
     return true;
 }
 
-bool snsGyroscopeDestroy()
+bool iGyroscopeDestroy()
 {
     pthread_mutex_lock(&mutexCb);
     cbGyroscope = 0;
@@ -62,83 +65,90 @@ bool snsGyroscopeDestroy()
 
 bool snsGyroscopeGetGyroscopeData(TGyroscopeData * gyroData)
 {
-    if(!gyroData)
+    bool retval = false;
+    if(gyroData)
     {
-    	return false;
+        pthread_mutex_lock(&mutexData);
+        *gyroData = gGyroscopeData;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
     }
-
-    pthread_mutex_lock(&mutexData);
-    *gyroData = gGyroscopeData;
-    pthread_mutex_unlock(&mutexData);
-
-    return true;
+    return retval;    
 }
 
 bool snsGyroscopeRegisterCallback(GyroscopeCallback callback)
 {
-    if(!callback)
-    {
-        return false;
-    }
+    bool retval = false;
 
-    //printf("snsGyroscopeRegisterCallback\n");
-    pthread_mutex_lock(&mutexCb);
-    if(cbGyroscope != 0) 
-    {   
-        //already registered
+    //only if valid callback and not already registered
+    if(callback && !cbGyroscope)
+    {
+        pthread_mutex_lock(&mutexCb);
+        cbGyroscope = callback;
         pthread_mutex_unlock(&mutexCb);
-        return false;
+        retval = true;
     }
-    cbGyroscope = callback;
-    pthread_mutex_unlock(&mutexCb);
-
-    return true;
+    return retval;
 }
 
 bool snsGyroscopeDeregisterCallback(GyroscopeCallback callback)
 {
-    if(!callback)
-    {
-        return false;
-    }
+    bool retval = false;
 
-    //printf("snsGyroscopeDeregisterCallback\n");
-    pthread_mutex_lock(&mutexCb);
-    if(cbGyroscope == callback)
-    { 
+    if((cbGyroscope == callback) && callback)
+    {
+        pthread_mutex_lock(&mutexCb);
         cbGyroscope = 0;
+        pthread_mutex_unlock(&mutexCb);
+        retval = true;
     }
-    pthread_mutex_unlock(&mutexCb);
 
-    return true;
+    return retval;
 }
 
 bool snsGyroscopeGetMetaData(TSensorMetaData *data)
 {
-    if(!data) 
+    bool retval = false;    
+    
+    if(data) 
     {
-        return false;
+        pthread_mutex_lock(&mutexData);
+        *data = gSensorsMetaData[1];
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
     }
-    
-    pthread_mutex_lock(&mutexData);
-    *data = gSensorsMetaData[1];
-    pthread_mutex_unlock(&mutexData);
 
-    return true;
+    return retval;
 }
 
 bool snsGyroscopeGetConfiguration(TGyroscopeConfiguration* gyroConfig)
 {
-    if(!gyroConfig) 
+    bool retval = false; 
+    if(gyroConfig) 
     {
-        return false;
+        pthread_mutex_lock(&mutexData);
+        *gyroConfig = gGyroscopeConfiguration;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
     }
 
-    pthread_mutex_lock(&mutexData);
-    *gyroConfig = gGyroscopeConfiguration;
-    pthread_mutex_unlock(&mutexData);
+    return retval;
+}
 
-    return true;
+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);
+    }
 }
 
 
diff --git a/sensors-service/src/mpu6050.cpp b/sensors-service/src/mpu6050.cpp
new file mode 100644
index 0000000..3e546dc
--- /dev/null
+++ b/sensors-service/src/mpu6050.cpp
@@ -0,0 +1,622 @@
+/**************************************************************************
+ * @brief Access library for MPU6050/MPU9150 inertial sensors
+ *
+ * @details Encapsulate I2C access to a MPU6050 sensor on a Linux machine
+ * The MPU6050 from Invense is a 6DOF inertial sensor
+ * @see http://www.invensense.com/mems/gyro/mpu6050.html
+ * The functions work also with the MPU9150 which is a MPU6050 with
+ * additional functionality (magnetometer)
+ * @see http://www.invensense.com/mems/gyro/mpu9150.html
+ *
+ * @author Helmut Schmidt <https://github.com/huirad>
+ * @copyright Copyright (C) 2015, Helmut Schmidt
+ *
+ * @license MPL-2.0 <http://spdx.org/licenses/MPL-2.0>
+ *
+ **************************************************************************/
+
+
+/** ===================================================================
+ * 1.) INCLUDES
+ */
+
+ //provided interface
+#include "mpu6050.h"
+
+//linux i2c access
+#include <linux/i2c-dev.h> //RPi: located in /usr/include/linux/i2c-dev.h - all functions inline
+
+//standard c library functions
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <time.h>
+#include <pthread.h>
+
+
+
+/** ===================================================================
+ * 2.) MPU6050 magic numbers
+ * Source: http://invensense.com/mems/gyro/documents/RM-MPU-6000A-00v4.2.pdf
+ */
+
+/** MPU6050 register addresses
+ *  Accelerometer, temperature, and gyro readings are each 16bit signed integers
+ *  stored in two consecutive registeres as 2's complement value
+ *  The registers MPU6050_REG_ACCEL_XOUT ... MPU6050_REG_GYRO_ZOUT
+ *  each contain the high byte of the 16 bit. The low by is in the next register
+ *  Favourably, the accelerometer, temperature, and gyro registers
+ *  are clustered in a fashion that is optimized for block reads
+ */
+#define MPU6050_REG_CONFIG     0x1A
+#define MPU6050_REG_ACCEL_XOUT 0x3B
+#define MPU6050_REG_ACCEL_YOUT 0x3D
+#define MPU6050_REG_ACCEL_ZOUT 0x3F
+#define MPU6050_REG_TEMP_OUT   0x41
+#define MPU6050_REG_GYRO_XOUT  0x43
+#define MPU6050_REG_GYRO_YOUT  0x45
+#define MPU6050_REG_GYRO_ZOUT  0x47
+#define MPU6050_REG_PWR_MGMT_1 0x6B
+#define MPU6050_REG_WHO_AM_I   0x75
+
+ /** MPU6050 register values
+  */
+#define MPU6050_PWR_MGMT_1__SLEEP  0x40
+#define MPU6050_PWR_MGMT_1__WAKEUP 0x00
+#define MPU6050_WHO_AM_I           0x68
+
+ /** MPU6050 conversion factors
+  * Accelerometer scale at default +-2g range: 16384 LSB/g
+  * Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53
+  * Gyroscope scale at default +-250 deg/s range: 131 LSB/(deg/s)
+  */
+#define MPU6050_ACCEL_SCALE  16384.0
+#define MPU6050_TEMP_SCALE   340.0
+#define MPU6050_TEMP_BIAS    36.53
+#define MPU6050_GYRO_SCALE   131.0
+
+
+/** ===================================================================
+ * 3.) PRIVATE VARIABLES AND FUNCTIONS
+ * Functions starting with i2c_ encapsulate the I2C bus access
+ * See
+ *   https://www.kernel.org/doc/Documentation/i2c/dev-interface
+ *   https://www.kernel.org/doc/Documentation/i2c/smbus-protocol
+ * Functions starting with conv_ convert the raw data to common measurement units
+ */
+
+/** Global file descriptor - must be initialized by calling i2c_mpu6050_init()
+ */
+static int _i2c_fd = -1;
+/** Global device address - must be initialized by calling i2c_mpu6050_init()
+ */
+static uint8_t _i2c_addr = 0;
+
+/** MPU6050 reader thread control
+ */
+volatile int _mpu6050_reader_loop = 0;
+pthread_t _reader_thread;
+uint64_t _sample_interval;
+uint16_t _num_samples;
+bool _average;
+
+/** Callback function and associated mutex
+ */
+pthread_mutex_t _mutex_cb  = PTHREAD_MUTEX_INITIALIZER;
+volatile MPU6050Callback _cb = 0;
+
+/** Write a 8 bit unsigned integer to a register
+ */
+static bool i2c_write_uint8(uint8_t reg, uint8_t data)
+{
+    bool result = false;
+    __s32 i2c_result;
+
+    if (_i2c_fd < 0)
+    {
+        /* Invalid file descriptor */
+    }
+    else
+    {
+        i2c_result = i2c_smbus_write_byte_data(_i2c_fd, reg, data);
+        if (i2c_result < 0)
+        {
+        /* ERROR HANDLING: i2c transaction failed */
+        }
+        else
+        {
+        result = true;
+        }
+    }
+    return result;
+}
+
+/** Read a 8 bit unsigned integer from a register
+ */
+static bool i2c_read_uint8(uint8_t reg, uint8_t* data)
+{
+    bool result = false;
+    __s32 i2c_result;
+
+    if (_i2c_fd < 0)
+    {
+        /* Invalid file descriptor */
+    }
+    else
+    {
+        /* Using SMBus commands */
+        i2c_result = i2c_smbus_read_byte_data(_i2c_fd, reg);
+        if (i2c_result < 0)
+        {
+            /* ERROR HANDLING: i2c transaction failed */
+        }
+        else
+        {
+            *data = (uint8_t) i2c_result;
+            //printf("Register 0x%02X: %08X = %d\n", reg, i2c_result, *data);
+            result = true;
+        }
+    }
+    return result;
+}
+
+
+/** Read a 16 bit signed integer from two consecutive registers
+ */
+static bool i2c_read_int16(uint8_t reg, int16_t* data)
+{
+    bool result = false;
+    __s32 i2c_result;
+    char buf[10];
+
+    if (_i2c_fd < 0)
+    {
+        /* Invalid file descriptor */
+    }
+    else
+    {
+        /* Using SMBus commands */
+        i2c_result = i2c_smbus_read_word_data(_i2c_fd, reg);
+        if (i2c_result < 0)
+        {
+            /* ERROR HANDLING: i2c transaction failed */
+        }
+        else
+        {
+            /* i2c_result contains the read word */
+            //swap bytes as i2c_smbus_read_word_data() expects low by first!
+            uint16_t tmp = ( ((i2c_result&0xFF)<<8) | ((i2c_result&0xFF00)>>8));
+            *data = (int16_t) tmp;
+            //printf("Register 0x%02X: %08X = %d\n", reg, i2c_result, *data);
+            result = true;
+        }
+    }
+    return result;
+}
+
+/** Read a block of 8 bit unsigned integers from two consecutive registers
+ */
+static bool i2c_read_block_1(uint8_t reg, uint8_t* data, uint8_t size)
+{
+    bool result = false;
+
+    if (_i2c_fd < 0)
+    {
+        /* Invalid file descriptor */
+    }
+    else
+    {
+        if (write(_i2c_fd, &reg, 1) == 1)
+        {
+            int8_t count = 0;
+            count = read(_i2c_fd, data, size);
+            if (count == size)
+            {
+                result = true;
+            }
+        }
+    }
+    return result;
+}
+
+/** Read a block of 8 bit unsigned integers from two consecutive registers
+ *  Variant using 1 single ioctl() call instead of 1 read() followed by 1 write()
+ *  See https://www.kernel.org/doc/Documentation/i2c/dev-interface
+ *  on ioctl(file, I2C_RDWR, struct i2c_rdwr_ioctl_data *msgset).
+ *  See also
+ *    [i2c_rdwr_ioctl_data] (http://lxr.free-electrons.com/source/include/uapi/linux/i2c-dev.h#L64)
+ *    [i2c_msg] (http://lxr.free-electrons.com/source/include/uapi/linux/i2c.h#L68)
+ * Seems to be marginally faster than i2c_read_block_1(): Ca 1% when reading 8 bytes
+ */
+static bool i2c_read_block_2(uint8_t reg, uint8_t* data, uint8_t size)
+{
+    bool result = false;
+    struct i2c_rdwr_ioctl_data i2c_data;
+    struct i2c_msg msg[2];
+    int i2c_result;
+
+    if (_i2c_fd < 0)
+    {
+        /* Invalid file descriptor */
+    }
+    else
+    {
+        i2c_data.msgs = msg;
+        i2c_data.nmsgs = 2;     // two i2c_msg
+
+        i2c_data.msgs[0].addr = _i2c_addr;
+        i2c_data.msgs[0].flags = 0;         // write
+        i2c_data.msgs[0].len = 1;           // only one byte
+        i2c_data.msgs[0].buf = (char*)&reg; // typecast to char*: see i2c-dev.h
+
+        i2c_data.msgs[1].addr = _i2c_addr;
+        i2c_data.msgs[1].flags = I2C_M_RD;  // read command
+        i2c_data.msgs[1].len = size;
+        i2c_data.msgs[1].buf = (char*)data; // typecast to char*: see i2c-dev.h
+
+        i2c_result = ioctl(_i2c_fd, I2C_RDWR, &i2c_data);
+
+        if (i2c_result < 0)
+        {
+            /* ERROR HANDLING: i2c transaction failed */
+        }
+        else
+        {
+            result = true;
+        }
+    }
+    return result;
+}
+
+static bool i2c_read_block(uint8_t reg, uint8_t* data, uint8_t size)
+{
+    return i2c_read_block_2(reg, data, size);
+}
+
+static bool i2c_mpu6050_init(const char* i2c_device, uint8_t i2c_addr)
+{
+    bool result = true;
+    _i2c_fd = open(i2c_device, O_RDWR);
+    if (_i2c_fd < 0)
+    {
+        /* ERROR HANDLING; you can check errno to see what went wrong */
+        result = false;
+    }
+    else
+    {
+        if (ioctl(_i2c_fd, I2C_SLAVE, i2c_addr) < 0)
+        {
+            /* ERROR HANDLING; you can check errno to see what went wrong */
+            result = false;
+        }
+        else
+        {
+            _i2c_addr = i2c_addr;
+        }
+    }
+    return result;
+}
+
+static bool i2c_mpu6050_deinit()
+{
+    bool result = false;
+    if (_i2c_fd < 0)
+    {
+        /* Invalid file descriptor */
+    }
+    else
+    {
+        close(_i2c_fd);
+        _i2c_fd = -1;
+        _i2c_addr = 0;
+        result = true;
+    }
+    return result;
+}
+
+static bool mpu6050_wakeup()
+{
+    uint8_t whoami;
+    bool result = true;
+    //Wake up the MPU6050 as it starts in sleep mode
+    result = i2c_write_uint8(MPU6050_REG_PWR_MGMT_1, MPU6050_PWR_MGMT_1__WAKEUP);
+    //Test the WHO_AM_I register
+    if (result)
+    {
+        result = i2c_read_uint8(MPU6050_REG_WHO_AM_I, &whoami);
+        result = result && (MPU6050_WHO_AM_I == whoami) ;
+    }
+    //wait 10ms to guarantee that sensor data is available at next read attempt
+    usleep(10000);
+    return result;
+}
+
+static bool mpu6050_setDLPF(EMPU6050LowPassFilterBandwidth bandwidth)
+{
+    bool result = true;
+    result = i2c_write_uint8(MPU6050_REG_CONFIG, bandwidth);
+    return result;
+}
+
+
+static float conv_accel(int16_t raw_accel)
+{
+    return raw_accel / MPU6050_ACCEL_SCALE;
+}
+
+static float conv_temp(int16_t raw_temp)
+{
+    return raw_temp / MPU6050_TEMP_SCALE + MPU6050_TEMP_BIAS;
+}
+
+static float conv_gyro(int16_t raw_gyro)
+{
+    return raw_gyro / MPU6050_GYRO_SCALE;
+}
+
+static uint64_t sleep_until(uint64_t wakeup)
+{
+    uint64_t start =  mpu6050_get_timestamp();
+
+    if (wakeup > start)
+    {
+        uint64_t diff = wakeup - start;
+        struct timespec t;
+        t.tv_sec = diff / 1000;
+        t.tv_nsec = (diff - t.tv_sec*1000) * 1000000;
+        while(nanosleep(&t, &t));
+    }
+
+    uint64_t stop =  mpu6050_get_timestamp();
+    return stop-start;
+}
+
+static bool fire_callback(const TMPU6050Vector3D acceleration[], const TMPU6050Vector3D gyro_angular_rate[], const float temperature[], const uint64_t timestamp[], const uint16_t num_elements, bool average)
+{
+    pthread_mutex_lock(&_mutex_cb);
+    if (_cb)
+    {
+        if (average)
+        {
+            TMPU6050Vector3D av_acceleration = acceleration[0];
+            TMPU6050Vector3D av_gyro_angular_rate = gyro_angular_rate[0];
+            float av_temperature = temperature[0];
+            for (uint16_t i=1; i<num_elements; i++)
+            {
+                av_acceleration.x += acceleration[i].x;
+                av_acceleration.y += acceleration[i].y;
+                av_acceleration.z += acceleration[i].z;
+                av_gyro_angular_rate.x += gyro_angular_rate[i].x;
+                av_gyro_angular_rate.y += gyro_angular_rate[i].y;
+                av_gyro_angular_rate.z += gyro_angular_rate[i].z;
+                av_temperature += temperature[i];
+            }
+            av_acceleration.x /= num_elements;
+            av_acceleration.y /= num_elements;
+            av_acceleration.z /= num_elements;
+            av_gyro_angular_rate.x /= num_elements;
+            av_gyro_angular_rate.y /= num_elements;
+            av_gyro_angular_rate.z /= num_elements;
+            av_temperature /= num_elements;
+            uint64_t last_timestamp = timestamp[num_elements-1];
+            _cb(&av_acceleration, &av_gyro_angular_rate, &av_temperature, &last_timestamp, 1);
+        }
+        else
+        {
+            _cb(acceleration, gyro_angular_rate, temperature, timestamp, num_elements);
+        }
+    }
+    pthread_mutex_unlock(&_mutex_cb);
+}
+
+/**
+ * Worker thread to read MPU6050 data
+ * @param param pointer to parameters (currently unused)
+ */
+static void* mpu6050_reader_thread(void* param)
+{
+    TMPU6050Vector3D acceleration[_num_samples];
+    TMPU6050Vector3D gyro_angular_rate[_num_samples];
+    float temperature[_num_samples];
+    uint64_t timestamp[_num_samples];
+
+    uint16_t sample_idx = 0;
+
+    uint64_t next =  mpu6050_get_timestamp();
+    uint64_t next_cb = next;
+
+    while (_mpu6050_reader_loop)
+    {
+        mpu6050_read_accel_gyro(&acceleration[sample_idx], &gyro_angular_rate[sample_idx], &temperature[sample_idx], &timestamp[sample_idx]);
+
+        sample_idx++;
+        //fire callback when either the requested number of samples has been acquired or the corresponding time is over
+        if ((sample_idx == _num_samples) || (mpu6050_get_timestamp() > mpu6050_get_timestamp()))
+        {
+            fire_callback(acceleration, gyro_angular_rate, temperature, timestamp, _num_samples, _average);
+            sample_idx = 0;
+            next_cb += _sample_interval*_num_samples;
+        }
+        //wait until next sampling timeslot
+        next = next + _sample_interval;
+        sleep_until(next);
+    }
+}
+
+
+
+
+/** ===================================================================
+ * 4.) FUNCTIONS IMPLEMENTING THE PUBLIC INTERFACE OF mpu6050.h
+ */
+
+bool mpu6050_init(const char* i2c_device, uint8_t i2c_addr, EMPU6050LowPassFilterBandwidth bandwidth)
+{
+    bool result = false;
+    result = i2c_mpu6050_init(i2c_device, i2c_addr);
+    if (result)
+    {
+        result = mpu6050_setDLPF(bandwidth);
+    }
+    if (result)
+    {
+        result = mpu6050_wakeup();
+    }
+    return result;
+}
+
+bool mpu6050_deinit()
+{
+    bool result = false;
+    result = i2c_mpu6050_deinit();
+    return result;
+}
+
+
+bool mpu6050_read_accel_gyro(TMPU6050Vector3D* acceleration, TMPU6050Vector3D* gyro_angular_rate, float* temperature, uint64_t* timestamp)
+{
+    bool result = true;
+    int16_t value;
+    struct timespec time_value;
+    uint8_t block[14];
+
+    //always read temperature
+    uint8_t start_reg = MPU6050_REG_TEMP_OUT;
+    uint16_t num_bytes = 2;
+    uint8_t start = 6;
+
+    //read acceleration?
+    if (acceleration)
+    {
+        start_reg = MPU6050_REG_ACCEL_XOUT;
+        num_bytes +=6;
+        start = 0;
+    }
+    //read gyro_angular_rate?
+    if (gyro_angular_rate)
+    {
+        num_bytes +=6;
+    }
+
+    if (timestamp != NULL)
+    {
+        *timestamp = mpu6050_get_timestamp();
+    }
+
+    if (i2c_read_block(start_reg, block+start, num_bytes))
+    {
+        if (acceleration != NULL)
+        {
+            value = (((int16_t)block[0]) << 8) | block[1];
+            acceleration->x = conv_accel(value);
+            value = (((int16_t)block[2]) << 8) | block[3];
+            acceleration->y = conv_accel(value);
+            value = (((int16_t)block[4]) << 8) | block[5];
+            acceleration->z = conv_accel(value);
+        }
+        if (temperature != NULL)
+        {
+            value = (((int16_t)block[6]) << 8) | block[7];
+            *temperature = conv_temp(value);
+        }
+        if (gyro_angular_rate != NULL)
+        {
+            value = (((int16_t)block[8]) << 8) | block[9];
+            gyro_angular_rate->x = conv_gyro(value);
+            value = (((int16_t)block[10]) << 8) | block[11];
+            gyro_angular_rate->y = conv_gyro(value);
+            value = (((int16_t)block[12]) << 8) | block[13];
+            gyro_angular_rate->z = conv_gyro(value);
+        }
+    }
+    else
+    {
+        result = false;
+    }
+    return result;
+}
+
+bool mpu6050_register_callback(MPU6050Callback callback)
+{
+    if(_cb != 0)
+    {
+        return false; //if already registered
+    }
+
+    pthread_mutex_lock(&_mutex_cb);
+    _cb = callback;
+    pthread_mutex_unlock(&_mutex_cb);
+
+    return true;
+}
+
+bool mpu6050_deregister_callback(MPU6050Callback callback)
+{
+    if(_cb == callback && _cb != 0)
+    {
+        return false; //if already registered
+    }
+
+    pthread_mutex_lock(&_mutex_cb);
+    _cb = 0;
+    pthread_mutex_unlock(&_mutex_cb);
+
+    return true;
+}
+
+bool mpu6050_start_reader_thread(uint64_t sample_interval, uint16_t num_samples, bool average)
+{
+    if (_mpu6050_reader_loop)
+    {
+        return false; //thread already running
+    }
+    if (sample_interval == 0)
+    {
+        return false;
+    }
+    if (num_samples == 0)
+    {
+        return false;
+    }
+
+    _sample_interval = sample_interval;
+    _num_samples = num_samples;
+    _average = average;
+
+    _mpu6050_reader_loop = 1;
+
+    int res = pthread_create (&_reader_thread, NULL, mpu6050_reader_thread, NULL);
+
+    if (res != 0)
+    {
+        _mpu6050_reader_loop = 0;
+        return false;
+    }
+
+    return true;
+}
+
+bool mpu6050_stop_reader_thread()
+{
+    _mpu6050_reader_loop = 0;
+    pthread_join (_reader_thread, NULL);
+    return true;
+}
+
+uint64_t mpu6050_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;
+  }
+}
+
diff --git a/sensors-service/src/mpu6050.h b/sensors-service/src/mpu6050.h
new file mode 100644
index 0000000..c646722
--- /dev/null
+++ b/sensors-service/src/mpu6050.h
@@ -0,0 +1,201 @@
+/**************************************************************************
+ * @brief Access library for MPU6050/MPU9150 inertial sensors 
+ *
+ * @details Encapsulate I2C access to a MPU6050 sensor on a Linux machine
+ * The MPU6050 from Invense is a 6DOF inertial sensor
+ * @see http://www.invensense.com/mems/gyro/mpu6050.html 
+ * The functions work also with the MPU9150 which is a MPU6050 with 
+ * additional functionality (magnetometer)
+ * @see http://www.invensense.com/mems/gyro/mpu9150.html
+ *
+ *
+ * @author Helmut Schmidt <https://github.com/huirad>
+ * @copyright Copyright (C) 2015, Helmut Schmidt
+ * 
+ * @license MPL-2.0 <http://spdx.org/licenses/MPL-2.0>
+ *
+ **************************************************************************/
+
+#ifndef INCLUDE_MPU6050
+#define INCLUDE_MPU6050
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+ 
+ 
+/** Part 0: I2C device names and device addresses
+ *
+ */
+
+/** Typical I2C device names on Linux machines
+  * For early Raspberry Pi machines, its typically "/dev/i2c-0"
+  * For most later (starting already Q3/2012) Raspberry Pi machines, its typically "/dev/i2c-1"
+  */
+#define MPU6050_I2C_DEV_0 "/dev/i2c-0"
+#define MPU6050_I2C_DEV_1 "/dev/i2c-1"
+#define MPU6050_I2C_DEV_2 "/dev/i2c-2"
+#define MPU6050_I2C_DEV_3 "/dev/i2c-3"
+#define MPU6050_I2C_DEV_DEFAULT MPU6050_I2C_DEV_1
+
+/** Possible I2C device addresses of the MPU6050
+  * 0x68 is the default address
+  */
+#define MPU6050_ADDR_1 0x68
+#define MPU6050_ADDR_2 0x69
+ 
+/** Digital low pass filter settings
+  * See the MPU6050 register map for details
+  * http://invensense.com/mems/gyro/documents/RM-MPU-6000A-00v4.2.pdf
+  */ 
+enum EMPU6050LowPassFilterBandwidth
+{
+    MPU6050_DLPF_256HZ = 0x00,    //Gyro: 256Hz,  Accel: 260Hz
+    MPU6050_DLPF_188HZ = 0x01,    //Gyro: 188Hz,  Accel: 184Hz
+    MPU6050_DLPF_98HZ  = 0x02,    //Gyro:  98Hz,  Accel:  94Hz
+    MPU6050_DLPF_42HZ  = 0x03,    //Gyro:  42Hz,  Accel:  44Hz
+    MPU6050_DLPF_20HZ  = 0x04,    //Gyro:  20Hz,  Accel:  21Hz
+    MPU6050_DLPF_10HZ  = 0x05,    //Gyro:  10Hz,  Accel:  10Hz
+    MPU6050_DLPF_5HZ   = 0x06     //Gyro:   5Hz,  Accel:   5Hz
+};
+
+ 
+/** Part 1: Functions to access the MPU6050
+ *
+ */
+  
+/**
+ * Container for quantities which can be expressed as 3 dimensional vector.
+ * The axis system is to be assumed cartesian and right-handed
+ * Measurement units depend on type of data:
+ *   For accelerometer measurements, the unit is g
+ *   For gyro measurements, the unit is degrees per seconds (deg/s)
+ */
+typedef struct 
+{
+  float x;        /**< Vector component of the quantity along the x-axis */
+  float y;        /**< Vector component of the quantity along the y-axis */
+  float z;        /**< Vector component of the quantity along the Z-axis */
+} TMPU6050Vector3D;
+  
+/**
+ * Initialize the MPU6050 access.
+ * Must be called before using any of the other functions.
+ * @param i2c_device the name of the i2c device on which the MPU6050 is attached
+ * @param i2c_addr the I2C address of the MPU6050
+ * @param bandwidth bandwidth of the digital low pass filter
+ * @return true on success.
+ */
+bool mpu6050_init(const char* i2c_device = MPU6050_I2C_DEV_DEFAULT, uint8_t i2c_addr=MPU6050_ADDR_1, EMPU6050LowPassFilterBandwidth bandwidth=MPU6050_DLPF_256HZ); 
+
+/**
+ * Release the MPU6050 access.
+ * @return true on success.
+ */
+bool mpu6050_deinit(); 
+
+/**
+ * Read the current acceleration, angular rate and temperature from the MPU6050
+ * Any pointer may be NULL to indicate that the corresponding data is not requested
+ * @param acceleration returns the acceleration vector. Measurement unit is g [1g = 9.80665 m/(s*s)]
+ * @param gyro_angular_rate returns the angular rate vector as measured by gyroscope . Measurement unit is deg/s [degrees per seconds]
+ * @param temperature returns the temperature in degrees celsius
+ * @param timestamp returns a system timestamp in ms (milliseconds) derived from clock_gettime(CLOCK_MONOTONIC);
+ * @return true on success.
+ * @note: Never call this function while the callback mechanism is running!
+ */
+bool mpu6050_read_accel_gyro(TMPU6050Vector3D* acceleration, TMPU6050Vector3D* gyro_angular_rate, float* temperature, uint64_t* timestamp);
+
+/** Part 2: Provide data via callback mechanism
+ *
+ */
+
+/**
+ * Callback type for MPU6050 data.
+ * Use this type of callback if you want to register for MPU6050 data.
+ * This callback may return buffered data (num_elements >1) depending on configuration
+ * If the arrays contain buffered data (num_elements >1), the elements will be ordered with rising timestamps
+ * Data in different arrays with the same index belong together
+ * @param acceleration pointer to an array of TMPU6050Vector3D with size num_elements containing acceleration data
+ * @param gyro_angular_rate pointer to an array of TMPU6050Vector3D with size num_elements containing angular rate data
+ * @param temperature pointer to an array of float with size num_elements containing temperature data
+ * @param timestamp pointer to an array of uint64_t with size num_elements containing timestamps
+ * @param num_elements: allowed range: >=1. If num_elements >1, buffered data are provided. 
+ */
+typedef void (*MPU6050Callback)(const TMPU6050Vector3D acceleration[], const TMPU6050Vector3D gyro_angular_rate[], const float temperature[], const uint64_t timestamp[], const uint16_t num_elements);
+
+/**
+ * Register MPU6050 callback.
+ * This is the recommended method for continuously accessing the MPU6050 data
+ * The callback will be invoked when new MPU6050 data is available and the reader thread is running.
+ * To start the reader thred, call mpu6050_start_reader_thread().
+ * @param callback The callback which should be registered.
+ * @return True if callback has been registered successfully.
+ */
+bool mpu6050_register_callback(MPU6050Callback callback);
+
+/**
+ * Deregister MPU6050 callback.
+ * After calling this method no new MPU6050 data will be delivered to the client.
+ * @param callback The callback which should be deregistered.
+ * @return True if callback has been deregistered successfully.
+ */
+bool mpu6050_deregister_callback(MPU6050Callback callback);
+
+/**
+ * Start the MPU6050 reader thread.
+ * This thread will call the callback function registered by mpu6050_register_callback()
+ * The thread may be started before of after registering the callback function
+ * @param sample_interval Interval in ms (milliseconds) at which MPU6050 data shall be read
+ * @param num_samples Number of samples to read for one call of the callback function
+ * @param average If true, the only the average (mean value) of the num_samples will be returned
+ * @return True on success.
+ * @note Be sure to select a meaningful combination of sample_interval and igital low pass filter bandwidth
+ */
+bool mpu6050_start_reader_thread(uint64_t sample_interval, uint16_t num_samples, bool average);
+
+/**
+ * Stop the MPU6050 reader thread.
+ * @return True on success.
+ */
+bool mpu6050_stop_reader_thread();
+
+/** Part 3: Utility functions and conversion factors
+ *
+ */
+
+ /** Unit conversion factors
+  *
+  * MPU6050_UNIT_1_G: Standard gravity: 1g = 9.80665 m/(s*s)
+  * @see http://en.wikipedia.org/wiki/Standard_gravity
+  * @see http://www.bipm.org/utils/common/pdf/si_brochure_8_en.pdf#page=51
+  * @see http://en.wikipedia.org/wiki/Gravitational_acceleration 
+  *
+  * MPU6050_UNIT_1_RAD_IN_DEG: Angle conversion from radian to degrees
+  */
+#ifndef M_PI
+//No more available in C-99
+#define MPU6050_PI (4.0*atan(1.0))
+//#define M_PI 3.141592653589793238462643
+#else
+#define MPU6050_PI M_PI
+#endif  
+#define MPU6050_UNIT_1_G 9.80665
+#define MPU6050_UNIT_1_RAD_IN_DEG (180.0/MPU6050_PI)
+
+/**
+ * Get system timestamp
+ * @return returns a system timestamp in ms (milliseconds) derived from clock_gettime(CLOCK_MONOTONIC);
+ */
+uint64_t mpu6050_get_timestamp(); 
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //INCLUDE_MPU6050
diff --git a/sensors-service/src/sns-use-iphone.c b/sensors-service/src/sns-use-iphone.c
index 92c9925..9e9e0c8 100644
--- a/sensors-service/src/sns-use-iphone.c
+++ b/sensors-service/src/sns-use-iphone.c
@@ -49,8 +49,6 @@
 DLT_DECLARE_CONTEXT(gContext);
 
 pthread_t listenerThread;
-pthread_mutex_t mutexCb;
-pthread_mutex_t mutexData;
 bool isRunning = true;
 
 void *listenForMessages( void *ptr );
@@ -99,7 +97,18 @@ void snsGetVersion(int *major, int *minor, int *micro)
     }
 }
 
-static bool processGVGYRO(char* data, TGyroscopeData* pGyroscopeData)
+bool snsGyroscopeInit()
+{
+    return iGyroscopeInit();
+}
+
+bool snsGyroscopeDestroy()
+{
+    return iGyroscopeDestroy();
+}
+
+
+static bool processGVGYRO(char* data)
 {
     long unsigned int timestamp = 0;
     float yawRate;
@@ -122,28 +131,30 @@ static bool processGVGYRO(char* data, TGyroscopeData* pGyroscopeData)
         return false;
     }
 
-    pGyroscopeData->timestamp = timestamp;
+    TGyroscopeData gyroscopeData    
+    
+    gyroscopeData.timestamp = timestamp;
 
     //LOG_INFO(gContext,"timestamp:%lu",timestamp);
 
     //angleYaw
-    pGyroscopeData->yawRate = yawRate;
-    pGyroscopeData->validityBits |= GYROSCOPE_CONFIG_ANGLEYAW_VALID;
-    LOG_INFO(gContext,"yawRate: %lf", pGyroscopeData->yawRate);
+    gyroscopeData.yawRate = yawRate;
+    gyroscopeData.validityBits |= GYROSCOPE_CONFIG_ANGLEYAW_VALID;
+    LOG_INFO(gContext,"yawRate: %lf", gyroscopeData.yawRate);
      
     //anglePitch
-    pGyroscopeData->pitchRate = pitchRate;
-    pGyroscopeData->validityBits |= GYROSCOPE_CONFIG_ANGLEPITCH_VALID;
-    LOG_INFO(gContext,"pitchRate: %lf", pGyroscopeData->pitchRate);
+    gyroscopeData.pitchRate = pitchRate;
+    gyroscopeData.validityBits |= GYROSCOPE_CONFIG_ANGLEPITCH_VALID;
+    LOG_INFO(gContext,"pitchRate: %lf", gyroscopeData.pitchRate);
 
     //angleRoll
-    pGyroscopeData->rollRate = rollRate;
-    pGyroscopeData->validityBits |= GYROSCOPE_CONFIG_ANGLEROLL_VALID;
-    LOG_INFO(gContext,"rollRate: %f", pGyroscopeData->rollRate);
+    gyroscopeData.rollRate = rollRate;
+    gyroscopeData.validityBits |= GYROSCOPE_CONFIG_ANGLEROLL_VALID;
+    LOG_INFO(gContext,"rollRate: %f", gyroscopeData.rollRate);
 
     if(cbGyroscope != 0)
     {
-        cbGyroscope(pGyroscopeData,1);
+        cbGyroscope(&gyroscopeData,1);
     }
 
     return true;
diff --git a/sensors-service/src/sns-use-mpu6050.cpp b/sensors-service/src/sns-use-mpu6050.cpp
new file mode 100644
index 0000000..3a8d670
--- /dev/null
+++ b/sensors-service/src/sns-use-mpu6050.cpp
@@ -0,0 +1,195 @@
+/**************************************************************************
+* @licence app begin@
+*
+* SPDX-License-Identifier: MPL-2.0
+*
+* \ingroup SensorsService
+* \author Helmut Schmidt <https://github.com/huirad>
+*
+* \copyright Copyright (C) 2015, 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@
+**************************************************************************/
+
+//APIs provided
+#include "sns-init.h"
+#include "acceleration.h"
+#include "gyroscope.h"
+
+//standard headers
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <unistd.h>
+
+//sns internals
+#include "globals.h"
+#include "log.h"
+#include "mpu6050.h"
+
+DLT_DECLARE_CONTEXT(gContext);
+
+//configuration parameters - may be set from outside
+
+//configure how to address the mpu6050
+#ifndef MPU6050_I2C_DEV
+#define MPU6050_I2C_DEV MPU6050_I2C_DEV_1
+#endif
+
+//sample interval (ms)
+#ifndef MPU6050_SAMPLE_INTERVAL
+#define MPU6050_SAMPLE_INTERVAL 10
+#endif
+//number of samples per callback
+#ifndef MPU6050_NUM_SAMPLES
+#define MPU6050_NUM_SAMPLES 10
+#endif
+//control whether samples are averaged for the callback
+#ifndef MPU6050_AVG_SAMPLES
+#define MPU6050_AVG_SAMPLES true
+#endif
+
+static volatile bool is_initialized = false;
+
+static void mpu6050_cb(const TMPU6050Vector3D acceleration[], const TMPU6050Vector3D gyro_angular_rate[], const float temperature[], const uint64_t timestamp[], const uint16_t num_elements)
+{
+    TAccelerationData accel[MPU6050_NUM_SAMPLES] = {0};
+    TGyroscopeData gyro[MPU6050_NUM_SAMPLES] = {0};
+    
+    for (uint16_t i=0; i<num_elements; i++)
+    {
+        accel[i].timestamp = timestamp[i];
+        accel[i].x = acceleration[i].x*MPU6050_UNIT_1_G;
+        accel[i].y = acceleration[i].y*MPU6050_UNIT_1_G;
+        accel[i].z = acceleration[i].z*MPU6050_UNIT_1_G;
+        accel[i].temperature = temperature[i];
+        accel[i].validityBits = ACCELERATION_X_VALID | ACCELERATION_Y_VALID |
+                                ACCELERATION_Z_VALID | ACCELERATION_TEMPERATURE_VALID;
+
+        gyro[i].timestamp = timestamp[i];
+        gyro[i].yawRate = gyro_angular_rate[i].z;
+        gyro[i].pitchRate = gyro_angular_rate[i].y;
+        gyro[i].rollRate = gyro_angular_rate[i].x;
+        gyro[i].temperature = temperature[i];
+        gyro[i].validityBits = GYROSCOPE_YAWRATE_VALID | GYROSCOPE_PITCHRATE_VALID |
+                               GYROSCOPE_ROLLRATE_VALID | GYROSCOPE_TEMPERATURE_VALID;
+                             
+    }
+    updateAccelerationData(accel, num_elements);
+    updateGyroscopeData(gyro, num_elements);
+    
+}
+
+bool snsInit()
+{
+    //nothing special to do
+    return true;
+}
+
+bool snsDestroy()
+{
+    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 snsGyroscopeInit()
+{
+    bool is_ok = false;
+    if (is_initialized)
+    {
+        is_ok = true;
+    }
+    else
+    {
+        is_ok = iGyroscopeInit();
+        if (is_ok)
+        {
+            //DLPF cut-off 42Hz fits best to 100Hz sample rate
+            is_ok = mpu6050_init(MPU6050_I2C_DEV, MPU6050_ADDR_1, MPU6050_DLPF_42HZ);
+        }
+        if (is_ok)
+        {    
+            is_ok = mpu6050_register_callback(&mpu6050_cb);
+        }        
+        if (is_ok)
+        {
+            is_ok = mpu6050_start_reader_thread(MPU6050_SAMPLE_INTERVAL, MPU6050_NUM_SAMPLES, MPU6050_AVG_SAMPLES);
+        }
+        is_initialized = is_ok;
+    }
+    return is_ok;
+}
+
+bool snsGyroscopeDestroy()
+{
+    bool is_ok = false;
+    if (!is_initialized)
+    {
+        is_ok = true;
+    }
+    else
+    {
+        is_initialized = false;
+        bool is_ok = mpu6050_stop_reader_thread();
+        is_ok = is_ok && mpu6050_deregister_callback(&mpu6050_cb);
+        is_ok = is_ok && mpu6050_deinit();
+        is_ok = is_ok && iGyroscopeDestroy();
+    }
+    return is_ok;    
+}
+
+bool snsAccelerationInit()
+{
+    //gyro is the master for mpu6050 - nothing further to initialize
+    return snsGyroscopeInit();
+}
+
+bool snsAccelerationDestroy()
+{
+    //gyro is the master for mpu6050 - nothing further to deeinit
+    return snsGyroscopeDestroy();
+}
+
+//+ some dummy implementations
+bool snsVehicleSpeedInit()
+{
+    return iVehicleSpeedInit();
+}
+
+bool snsVehicleSpeedDestroy()
+{
+    return iVehicleSpeedDestroy();
+}
+
+bool snsWheeltickInit()
+{
+    return iWheeltickInit();
+}
+
+bool snsWheeltickDestroy()
+{
+    return iWheeltickDestroy();
+}
diff --git a/sensors-service/src/sns-use-replayer.c b/sensors-service/src/sns-use-replayer.c
index 3dbd1a3..de5ef41 100644
--- a/sensors-service/src/sns-use-replayer.c
+++ b/sensors-service/src/sns-use-replayer.c
@@ -16,6 +16,10 @@
 * @licence end@
 **************************************************************************/
 
+#include "sns-init.h"
+#include "acceleration.h"
+#include "gyroscope.h"
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <stdint.h>
@@ -33,7 +37,6 @@
 #include <memory.h>
 
 #include "globals.h"
-#include "sns-init.h"
 #include "log.h"
 
 #define BUFLEN 256
@@ -45,8 +48,6 @@
 DLT_DECLARE_CONTEXT(gContext);
 
 pthread_t listenerThread;
-pthread_mutex_t mutexCb;
-pthread_mutex_t mutexData;
 bool isRunning = false;
 
 void *listenForMessages( void *ptr );
@@ -94,7 +95,47 @@ void snsGetVersion(int *major, int *minor, int *micro)
     }
 }
 
-bool processGVSNSWHTK(char* data, TWheelticks* pWheelticks)
+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 snsWheeltickInit()
+{
+    return iWheeltickInit();
+}
+
+bool snsWheeltickDestroy()
+{
+    return iWheeltickDestroy();
+}
+
+bool processGVSNSWHTK(char* data)
 {
     //parse data like: 061076000,0$GVSNSWHTK,061076000,7,266,8,185,0,0,0,0
     
@@ -108,7 +149,7 @@ bool processGVSNSWHTK(char* data, TWheelticks* pWheelticks)
     TWheelticks whtk = { 0 };
     uint32_t n = 0;
 
-    if(!data || !pWheelticks)
+    if(!data)
     {
         LOG_ERROR_MSG(gContext,"wrong parameter!");
         return false;
@@ -127,8 +168,6 @@ bool processGVSNSWHTK(char* data, TWheelticks* pWheelticks)
         return false;
     }
 
-    *pWheelticks = whtk;
-
     //buffered data handling
     if (countdown < MAX_BUF_MSG) //enough space in buffer?
     {
@@ -155,9 +194,9 @@ bool processGVSNSWHTK(char* data, TWheelticks* pWheelticks)
         last_countdown = 0;
     }
 
-    if((cbWheelticks != 0) && (countdown == 0) && (buf_size >0) )
+    if((countdown == 0) && (buf_size >0) )
     {
-        cbWheelticks(buf_whtk,buf_size);
+        updateWheelticks(buf_whtk,buf_size);
         buf_size = 0;
         last_countdown = 0;        
     }
@@ -165,7 +204,7 @@ bool processGVSNSWHTK(char* data, TWheelticks* pWheelticks)
     return true;
 }
 
-static bool processGVSNSGYRO(char* data, TGyroscopeData* pGyroscopeData)
+static bool processGVSNSGYRO(char* data)
 {
     //parse data like: 061074000,0$GVSNSGYRO,061074000,-38.75,0,0,0,0X01
     
@@ -179,7 +218,7 @@ static bool processGVSNSGYRO(char* data, TGyroscopeData* pGyroscopeData)
     TGyroscopeData gyro = { 0 };
     uint32_t n = 0;
 
-    if(!data || !pGyroscopeData)
+    if(!data )
     {
         LOG_ERROR_MSG(gContext,"wrong parameter!");
         return false;
@@ -193,8 +232,6 @@ static bool processGVSNSGYRO(char* data, TGyroscopeData* pGyroscopeData)
         return false;
     }
 
-    *pGyroscopeData = gyro;
-
     //buffered data handling
     if (countdown < MAX_BUF_MSG) //enough space in buffer?
     {
@@ -221,9 +258,9 @@ static bool processGVSNSGYRO(char* data, TGyroscopeData* pGyroscopeData)
         last_countdown = 0;
     }
 
-    if((cbGyroscope != 0) && (countdown == 0) && (buf_size >0) )
+    if((countdown == 0) && (buf_size >0) )
     {
-        cbGyroscope(buf_gyro,buf_size);
+        updateGyroscopeData(buf_gyro,buf_size);
         buf_size = 0;
         last_countdown = 0;        
     }
@@ -233,7 +270,7 @@ static bool processGVSNSGYRO(char* data, TGyroscopeData* pGyroscopeData)
 
 
 
-static bool processGVSNSVEHSP(char* data, TVehicleSpeedData* pVehicleSpeedData)
+static bool processGVSNSVEHSP(char* data)
 {
     //parse data like: 061074000,0$GVSNSVEHSP,061074000,0.51,0X01
     
@@ -247,7 +284,7 @@ static bool processGVSNSVEHSP(char* data, TVehicleSpeedData* pVehicleSpeedData)
     TVehicleSpeedData vehsp = { 0 };
     uint32_t n = 0;
 
-    if(!data || !pVehicleSpeedData)
+    if(!data)
     {
         LOG_ERROR_MSG(gContext,"wrong parameter!");
         return false;
@@ -261,8 +298,6 @@ static bool processGVSNSVEHSP(char* data, TVehicleSpeedData* pVehicleSpeedData)
         return false;
     }
 
-    *pVehicleSpeedData = vehsp;
-
     //buffered data handling
     if (countdown < MAX_BUF_MSG) //enough space in buffer?
     {
@@ -289,9 +324,9 @@ static bool processGVSNSVEHSP(char* data, TVehicleSpeedData* pVehicleSpeedData)
         last_countdown = 0;
     }
 
-    if((cbVehicleSpeed != 0) && (countdown == 0) && (buf_size >0) )
+    if((countdown == 0) && (buf_size >0) )
     {
-        cbVehicleSpeed(buf_vehsp,buf_size);
+        updateVehicleSpeedData(buf_vehsp,buf_size);
         buf_size = 0;
         last_countdown = 0;        
     }
@@ -334,45 +369,58 @@ void *listenForMessages( void *ptr )
 
     while(isRunning == true)
     {
-        readBytes = recvfrom(s, buf, BUFLEN, 0, (struct sockaddr *)&si_other, &slen);
-
-        if(readBytes < 0)
+        //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)
         {
-            LOG_ERROR_MSG(gContext,"recvfrom() failed!");
-            exit(EXIT_FAILURE);
-        }
-        
-        buf[readBytes] = '\0';
-
-        LOG_DEBUG_MSG(gContext,"------------------------------------------------");
-
-        LOG_DEBUG(gContext,"Received Packet from %s:%d", 
-                  inet_ntoa(si_other.sin_addr), ntohs(si_other.sin_port));
-
-        sscanf(buf, "%*[^'$']$%[^',']", msgId);
-
-        LOG_DEBUG(gContext,"MsgID:%s",msgId);
-        LOG_DEBUG(gContext,"Len:%d",(int)strlen(buf));
-        LOG_INFO(gContext,"Data:%s",buf);
-
-        LOG_DEBUG_MSG(gContext,"------------------------------------------------");
-
-        pthread_mutex_lock(&mutexData);
+            
+            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';
+
+            LOG_DEBUG_MSG(gContext,"------------------------------------------------");
+    
+            LOG_DEBUG(gContext,"Received Packet from %s:%d", 
+                      inet_ntoa(si_other.sin_addr), ntohs(si_other.sin_port));
 
-        if(strcmp("GVSNSGYRO", msgId) == 0)
-        {
-            processGVSNSGYRO(buf, &gGyroscopeData);
-        }
-        else if(strcmp("GVSNSWHTK", msgId) == 0)
-        {
-            processGVSNSWHTK(buf, &gWheelticks);
-        }
-        else if(strcmp("GVSNSVEHSP", msgId) == 0)
-        {
-            processGVSNSVEHSP(buf, &gVehicleSpeedData);
+            sscanf(buf, "%*[^'$']$%[^',']", msgId);
+    
+            LOG_DEBUG(gContext,"MsgID:%s",msgId);
+            LOG_DEBUG(gContext,"Len:%d",(int)strlen(buf));
+            LOG_INFO(gContext,"Data:%s",buf);
+
+            LOG_DEBUG_MSG(gContext,"------------------------------------------------");
+
+            if(strcmp("GVSNSGYRO", msgId) == 0)
+            {
+                processGVSNSGYRO(buf);
+            }
+            else if(strcmp("GVSNSWHTK", msgId) == 0)
+            {
+                processGVSNSWHTK(buf);
+            }
+            else if(strcmp("GVSNSVEHSP", msgId) == 0)
+            {
+                processGVSNSVEHSP(buf);
+            }
         }
-
-        pthread_mutex_unlock(&mutexData);
     }
 
     close(s);
diff --git a/sensors-service/src/vehicle-speed.c b/sensors-service/src/vehicle-speed.c
index bd44023..c4379f4 100644
--- a/sensors-service/src/vehicle-speed.c
+++ b/sensors-service/src/vehicle-speed.c
@@ -15,10 +15,13 @@
 #include "vehicle-speed.h"
 #include "sns-meta-data.h"
 
-VehicleSpeedCallback cbVehicleSpeed = 0;
-TVehicleSpeedData gVehicleSpeedData;
+static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the callbacks
+static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
 
-bool snsVehicleSpeedInit()
+static volatile VehicleSpeedCallback cbVehicleSpeed = 0;
+static TVehicleSpeedData gVehicleSpeedData;
+
+bool iVehicleSpeedInit()
 {
     pthread_mutex_lock(&mutexCb);
     cbVehicleSpeed = 0;
@@ -27,7 +30,7 @@ bool snsVehicleSpeedInit()
     return true;
 }
 
-bool snsVehicleSpeedDestroy()
+bool iVehicleSpeedDestroy()
 {
     pthread_mutex_lock(&mutexCb);
     cbVehicleSpeed = 0;
@@ -52,57 +55,61 @@ bool snsVehicleSpeedGetVehicleSpeedData(TVehicleSpeedData* vehicleSpeed)
 
 bool snsVehicleSpeedRegisterCallback(VehicleSpeedCallback callback)
 {
-    if(!callback)
-    {
-        return false;
-    }
+    bool retval = false;
 
-    //printf("snsVehicleSpeedRegisterCallback\n");
-    pthread_mutex_lock(&mutexCb);
-    if(cbVehicleSpeed != 0) 
+    //only if valid callback and not already registered
+    if(callback && !cbVehicleSpeed)
     {
-        //already registered
+        pthread_mutex_lock(&mutexCb);
+        cbVehicleSpeed = callback;
         pthread_mutex_unlock(&mutexCb);
-        return false;
+        retval = true;
     }
-    cbVehicleSpeed = callback;
-    pthread_mutex_unlock(&mutexCb);
-
-    return true;
+    return retval;    
 }
 
 bool snsVehicleSpeedDeregisterCallback(VehicleSpeedCallback callback)
 {
-    if(!callback)
-    {
-        return false;
-    }
+    bool retval = false;
 
-    //printf("snsGyroscopeDeregisterCallback\n");
-    pthread_mutex_lock(&mutexCb);
-    if(cbVehicleSpeed == callback)
+    if((cbVehicleSpeed == callback) && callback)
     {
+        pthread_mutex_lock(&mutexCb);
         cbVehicleSpeed = 0;
+        pthread_mutex_unlock(&mutexCb);
+        retval = true;
     }
-    pthread_mutex_unlock(&mutexCb);
 
-    return false;
+    return retval;
 }
 
 bool snsVehicleSpeedGetMetaData(TSensorMetaData *data)
 {
-    if(!data) 
+    bool retval = false;    
+    
+    if(data) 
     {
-        return false;
+        pthread_mutex_lock(&mutexData);
+        *data = gSensorsMetaData[2];
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
     }
-    
-    pthread_mutex_lock(&mutexData);
-    *data = gSensorsMetaData[2];
-    pthread_mutex_unlock(&mutexData);
 
-    return 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);
+    }
+}
diff --git a/sensors-service/src/wheeltick.c b/sensors-service/src/wheeltick.c
index 99422ea..972b80f 100644
--- a/sensors-service/src/wheeltick.c
+++ b/sensors-service/src/wheeltick.c
@@ -19,10 +19,13 @@
 #include "globals.h"
 #include "wheel.h"
 
-WheeltickCallback cbWheelticks = 0;
-TWheelticks gWheelticks;
+static pthread_mutex_t mutexCb  = PTHREAD_MUTEX_INITIALIZER;   //protects the callbacks
+static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER;  //protects the data
 
-bool snsWheeltickInit()
+static volatile WheeltickCallback cbWheelticks = 0;
+static TWheelticks gWheelticks;
+
+bool iWheeltickInit()
 {
     int i;
 
@@ -41,7 +44,7 @@ bool snsWheeltickInit()
     return true;
 }
 
-bool snsWheeltickDestroy()
+bool iWheeltickDestroy()
 {
     pthread_mutex_lock(&mutexCb);
     cbWheelticks = 0;
@@ -52,53 +55,59 @@ bool snsWheeltickDestroy()
 
 bool snsWheeltickGetWheelticks(TWheelticks * ticks)
 {
-    if(!ticks)
+    bool retval = false;
+    if(ticks)
     {
-        return false;
+        pthread_mutex_lock(&mutexData);
+        *ticks = gWheelticks;
+        pthread_mutex_unlock(&mutexData);
+        retval = true;
     }
-
-    pthread_mutex_lock(&mutexData);
-    *ticks = gWheelticks;
-    pthread_mutex_unlock(&mutexData);
-
-    return true;
+    return retval;    
 }
 
 bool snsWheeltickRegisterCallback(WheeltickCallback callback)
 {
-    if(!callback)
-    {
-        return false;
-    }
+    bool retval = false;
 
-    //printf("snsWheeltickRegisterCallback\n");
-    pthread_mutex_lock(&mutexCb);
-    if(cbWheelticks != 0) 
+    //only if valid callback and not already registered
+    if(callback && !cbWheelticks)
     {
-        //already registered
+        pthread_mutex_lock(&mutexCb);
+        cbWheelticks = callback;
         pthread_mutex_unlock(&mutexCb);
-        return false; 
+        retval = true;
     }
-    cbWheelticks = callback;
-    pthread_mutex_unlock(&mutexCb);
-
-    return true;
+    return retval;
 }
 
 bool snsWheeltickDeregisterCallback(WheeltickCallback callback)
 {
-    if(!callback)
+    bool retval = false;
+
+    if((cbWheelticks == callback) && callback)
     {
-        return false;
+        pthread_mutex_lock(&mutexCb);
+        cbWheelticks = 0;
+        pthread_mutex_unlock(&mutexCb);
+        retval = true;
     }
 
-    //printf("snsWheeltickDeregisterCallback\n");
-    pthread_mutex_lock(&mutexCb);
-    if(cbWheelticks == callback)
-    { 
-        cbWheelticks = 0;      
-    }
-    pthread_mutex_unlock(&mutexCb);  
+    return retval;    
+}
 
-    return true;
+void updateWheelticks(const TWheelticks ticks[], uint16_t numElements)
+{
+    if (ticks != NULL && numElements > 0)
+    {
+        pthread_mutex_lock(&mutexData);
+        gWheelticks = ticks[numElements-1];
+        pthread_mutex_unlock(&mutexData);
+        pthread_mutex_lock(&mutexCb);
+        if (cbWheelticks)
+        {
+            cbWheelticks(ticks, numElements);
+        }
+        pthread_mutex_unlock(&mutexCb);
+    }
 }
diff --git a/sensors-service/test/CMakeLists.txt b/sensors-service/test/CMakeLists.txt
index 3569866..87244b2 100644
--- a/sensors-service/test/CMakeLists.txt
+++ b/sensors-service/test/CMakeLists.txt
@@ -32,6 +32,11 @@ find_package(PkgConfig)
 
 if(WITH_IPHONE)
     set(LIBRARIES sensors-service-use-iphone)
+elseif(WITH_MPU6050)
+    set(LIBRARIES sensors-service-use-mpu6050)
+    #for glibc <2.17, clock_gettime is in librt: http://linux.die.net/man/2/clo$
+    #TODO: is there a nice way to detect glibc version in CMake?
+    set(LIBRARIES ${LIBRARIES} rt)
 elseif(WITH_REPLAYER)
     set(LIBRARIES sensors-service-use-replayer)
 else()