From f76cb82f8afd0b66fb48cc4f1db081acc63b1b62 Mon Sep 17 00:00:00 2001 From: Helmut Schmidt Date: Wed, 28 Oct 2015 17:26:40 +0100 Subject: Positioning PoC: rework register functions, improve NMEA parser, add logger, add support for real gyro/accelerometer sensor --- sensors-service/CMakeLists.txt | 3 + sensors-service/src/CMakeLists.txt | 16 + sensors-service/src/acceleration.c | 108 +++--- sensors-service/src/globals.h | 32 +- sensors-service/src/gyroscope.c | 108 +++--- sensors-service/src/mpu6050.cpp | 622 ++++++++++++++++++++++++++++++++ sensors-service/src/mpu6050.h | 201 +++++++++++ sensors-service/src/sns-use-iphone.c | 39 +- sensors-service/src/sns-use-mpu6050.cpp | 195 ++++++++++ sensors-service/src/sns-use-replayer.c | 162 ++++++--- sensors-service/src/vehicle-speed.c | 79 ++-- sensors-service/src/wheeltick.c | 79 ++-- sensors-service/test/CMakeLists.txt | 5 + 13 files changed, 1400 insertions(+), 249 deletions(-) create mode 100644 sensors-service/src/mpu6050.cpp create mode 100644 sensors-service/src/mpu6050.h create mode 100644 sensors-service/src/sns-use-mpu6050.cpp (limited to 'sensors-service') 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 + * @copyright Copyright (C) 2015, Helmut Schmidt + * + * @license MPL-2.0 + * + **************************************************************************/ + + +/** =================================================================== + * 1.) INCLUDES + */ + + //provided interface +#include "mpu6050.h" + +//linux i2c access +#include //RPi: located in /usr/include/linux/i2c-dev.h - all functions inline + +//standard c library functions +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + +/** =================================================================== + * 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, ®, 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*)® // 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 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 + * @copyright Copyright (C) 2015, Helmut Schmidt + * + * @license MPL-2.0 + * + **************************************************************************/ + +#ifndef INCLUDE_MPU6050 +#define INCLUDE_MPU6050 + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + + +/** 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 +* +* \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 +#include +#include +#include +#include + +//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 #include #include @@ -33,7 +37,6 @@ #include #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() -- cgit v1.2.1