diff options
36 files changed, 3420 insertions, 274 deletions
diff --git a/CMakeLists.txt b/CMakeLists.txt index bf20060..7110d50 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,8 @@ # Update (2015/08/19) : Marco Residori <marco_residori@mentor.com> # - Added option WITH_FRANCA_SOMEIP_INTERFACE # - Renamed WITH_FRANCA_INTERFACE to WITH_FRANCA_DBUS_INTERFACE +# Update (2015/10/28) : Helmut Schmidt <Helmut.3.Schmidt@continental-corporation.com> +# - Add option for logger # @licence end@ ########################################################################### cmake_minimum_required(VERSION 2.6.0) @@ -28,6 +30,8 @@ option(WITH_GNSS_SERVICE "Build the GNSS Service" ON) option(WITH_LOG_REPLAYER "Build the Log Replayer" ON) +option(WITH_LOGGER + "Build the Logger" ON) option(WITH_SENSORS_SERVICE "Build the Sensors Service" ON) option(WITH_FRANCA_DBUS_INTERFACE @@ -53,6 +57,10 @@ if(WITH_LOG_REPLAYER) add_subdirectory(log-replayer) endif(WITH_LOG_REPLAYER) +if(WITH_LOGGER) + add_subdirectory(logger) +endif(WITH_LOGGER) + if(WITH_SENSORS_SERVICE) add_subdirectory(sensors-service) endif(WITH_SENSORS_SERVICE) @@ -59,6 +59,13 @@ To build with tests and reading GPS NMEA data from a GPS receiver attached to /d cmake -DWITH_NMEA=ON -DWITH_TESTS=ON -DWITH_DEBUG=ON -DGNSS_DEVICE=\"/dev/ttyACM0\" -DGNSS_BAUDRATE=B38400 ../ make +To build with tests and with the logger but without the EnhancedPositionService: +and reading GPS NMEA data from a GPS receiver attached to /dev/ttyACM0 at 38400 baud +with a u-blox chipset and 400ms transmission delay +and with MPU6050 as source for gyro and accelerometer data +cmake -DWITH_ENHANCED_POSITION_SERVICE=OFF -DWITH_NMEA=ON -DWITH_MPU6050=ON -DWITH_LOGGER=ON -DWITH_TESTS=ON -DWITH_DEBUG=ON -DGNSS_DEVICE=\"/dev/ttyACM0\" -DGNSS_BAUDRATE=B38400 -DGNSS_CHIPSET=UBLOX -DGNSS_DELAY=400 ../ +make + =============================== Compiler Options and default setting (see dependencies below) @@ -115,6 +122,16 @@ service: =============================== Dependencies =============================== +You might have to install additional packages to Compile and run the +Positioning PoC. +This section tries to summarize those dependencies. +Also necessary commands to install those packages are provided for Debian +based systems (Ubuntu, Raspbian, ...) with the apt package manager. +Other Linux distributions (e.g. openSuSE) may require some adaptation as +the package names and even the package manager may be different. + +CMake is used to create the make files +sudo apt-get install cmake DWITH_GPSD=ON requires that the package 'gpsd' is installed. To install 'gpsd', please execute the following command: @@ -123,6 +140,7 @@ sudo apt-get install gpsd libgps-dev To test the enhanced-position-service (dbus-service) the packages 'libdbus-1-dev', libdbus-c++-dev' and 'xsltproc' must be installed. To install these packages, please execute the following command: sudo apt-get install libdbus-1-dev libdbus-c++-dev xsltproc +sudo ldconfig To test the enhanced-position-service (commonapi-service) the package CommonAPI and CommonAPI code generators must be installed. Please see: @@ -151,5 +169,7 @@ DWITH_IPHONE=ON requires that the iPhone app 'SensorLogger' is installed on a iPhone and that the option to broadcast the sensor data is activated (port=5555). - +DWITH_MPU6050=ON requires that the header file for I2C access +(i2c-dev.h) is available on your system. +This header file is typically part of the i2c-tools package. diff --git a/enhanced-position-service/dbus/CMakeLists.txt b/enhanced-position-service/dbus/CMakeLists.txt index 86aa509..4b6956e 100644 --- a/enhanced-position-service/dbus/CMakeLists.txt +++ b/enhanced-position-service/dbus/CMakeLists.txt @@ -55,6 +55,8 @@ set(sensors-service_LIBRARY_DIRS "${PROJECT_BINARY_DIR}/../../sensors-service/sr if(WITH_IPHONE) set(sensors-service_LIBRARIES "sensors-service-use-iphone") +elseif(WITH_MPU6050) + set(sensors-service_LIBRARIES "sensors-service-use-mpu6050") elseif(WITH_REPLAYER) set(sensors-service_LIBRARIES "sensors-service-use-replayer") else() diff --git a/gnss-service/src/CMakeLists.txt b/gnss-service/src/CMakeLists.txt index 0cde3cc..c5693c4 100644 --- a/gnss-service/src/CMakeLists.txt +++ b/gnss-service/src/CMakeLists.txt @@ -58,6 +58,12 @@ elseif(WITH_NMEA) #generate library using NMEA parser as input add_definitions(-DGNSS_DEVICE=${GNSS_DEVICE}) add_definitions(-DGNSS_BAUDRATE=${GNSS_BAUDRATE}) + if(GNSS_CHIPSET) + add_definitions(-DGNSS_CHIPSET_${GNSS_CHIPSET}) + endif(GNSS_CHIPSET) + if(GNSS_DELAY) + add_definitions(-DGNSS_DELAY=${GNSS_DELAY}) + endif(GNSS_DELAY) set(LIB_SRC_USE_NMEA ${CMAKE_CURRENT_SOURCE_DIR}/gnss-use-nmea.cpp ${CMAKE_CURRENT_SOURCE_DIR}/hnmea.cpp ${CMAKE_CURRENT_SOURCE_DIR}/gnss-impl.c @@ -65,6 +71,9 @@ elseif(WITH_NMEA) add_library(gnss-service-use-nmea SHARED ${LIB_SRC_USE_NMEA}) target_link_libraries(gnss-service-use-nmea pthread ${LIBRARIES}) install(TARGETS gnss-service-use-nmea DESTINATION lib) + #for glibc <2.17, clock_gettime is in librt: http://linux.die.net/man/2/clock_gettime + #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}/gnss-use-replayer.c diff --git a/gnss-service/src/gnss-impl.c b/gnss-service/src/gnss-impl.c index 82e572d..5f183b5 100644 --- a/gnss-service/src/gnss-impl.c +++ b/gnss-service/src/gnss-impl.c @@ -19,18 +19,17 @@ #include "globals.h" #include "gnss.h" -pthread_mutex_t mutexCb = PTHREAD_MUTEX_INITIALIZER; //protects the callbacks -pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER; //protects the data +static pthread_mutex_t mutexCb = PTHREAD_MUTEX_INITIALIZER; //protects the callbacks +static pthread_mutex_t mutexData = PTHREAD_MUTEX_INITIALIZER; //protects the data +static TGNSSSatelliteDetail gSatelliteDetail; //TODO: buffer full set of satellite details for one point in time +static GNSSSatelliteDetailCallback cbSatelliteDetail = 0; -TGNSSSatelliteDetail gSatelliteDetail; //TODO: buffer full set of satellite details for one point in time -GNSSSatelliteDetailCallback cbSatelliteDetail = 0; +static TGNSSPosition gPosition; +static volatile GNSSPositionCallback cbPosition = 0; -TGNSSPosition gPosition; -volatile GNSSPositionCallback cbPosition = 0; - -TGNSSTime gTime; -GNSSTimeCallback cbTime = 0; +static TGNSSTime gTime; +static volatile GNSSTimeCallback cbTime = 0; diff --git a/gnss-service/src/gnss-use-nmea.cpp b/gnss-service/src/gnss-use-nmea.cpp index 9c5d11e..c771337 100644 --- a/gnss-service/src/gnss-use-nmea.cpp +++ b/gnss-service/src/gnss-use-nmea.cpp @@ -24,6 +24,9 @@ * * @licence end@ **************************************************************************/ +//for integer format macros such as PRIu64 +#define __STDC_FORMAT_MACROS +#include <inttypes.h> //provided interface #include "gnss-init.h" @@ -47,6 +50,23 @@ //the NMEA parser #include "hnmea.h" +/** + * CONFIGURATION PARAMETERS + * + * #required + * GNSS_DEVICE: device name at which GNSS receiver is attached, e.g. "dev/ttyACM0" + * GNSS_BAUDRATE: baud rate of GNSS receiver at device GNSS_DEVICE, e.g. B38400 + * + * #optional + * GNSS_CHIPSET_XXX: Identification of GNSS chipset, e.g. GNSS_CHIPSET_UBLOX + * GNSS_DELAY: Delay in ms of terminating NMEA sentence with respect to time of fix + * + */ +#ifndef GNSS_DELAY +#define GNSS_DELAY 0 +#endif + + DLT_DECLARE_CONTEXT(gContext); /** @@ -143,8 +163,17 @@ bool extractPosition(const GPS_DATA& gps_data, uint64_t timestamp, TGNSSPosition } gnss_pos.trackedSatellites = 9999; //not available gnss_pos.visibleSatellites = 9999; //not available - gnss_pos.sigmaHPosition = 9999; //not available - gnss_pos.sigmaAltitude = 9999; //not available + if (gps_data.valid & GPS_DATA_HACC) + { + gnss_pos.sigmaHPosition = gps_data.hacc; + gnss_pos.validityBits |= GNSS_POSITION_SHPOS_VALID; + } + if (gps_data.valid & GPS_DATA_HACC) + { + gnss_pos.sigmaAltitude = gps_data.vacc; + gnss_pos.validityBits |= GNSS_POSITION_SALT_VALID; + } + gnss_pos.sigmaHSpeed = 9999; //not available gnss_pos.sigmaVSpeed = 9999; //not available gnss_pos.sigmaHeading = 9999; //not available @@ -211,7 +240,7 @@ bool extractTime(const GPS_DATA& gps_data, uint64_t timestamp, TGNSSTime& gnss_t * @ref http://tldp.org/HOWTO/Serial-Programming-HOWTO/x115.html * @param gps_device [IN] device, e.g. "/dev/ttyACM0" * @param baudrate [IN] baud rate (see definitions in <asm/termbits.h> - * @return file descriptor of GNSS device + * @return file descriptor of GNSS device, negative values indicate an error */ int open_GNSS_NMEA_device(const char* gps_device, unsigned int baudrate) { @@ -225,7 +254,7 @@ int open_GNSS_NMEA_device(const char* gps_device, unsigned int baudrate) fd = open(gps_device, O_RDWR | O_NOCTTY ); if (fd <0) { - LOG_ERROR(gContext,"Cannot open: %s", gps_device); + //LOG_ERROR(gContext,"Cannot open: %s", gps_device); return fd; } @@ -293,7 +322,7 @@ int open_GNSS_NMEA_device(const char* gps_device, unsigned int baudrate) /* Done */ - LOG_DEBUG(gContext, "OPEN successful %d\n", fd); + //LOG_DEBUG(gContext, "OPEN successful %d\n", fd); return fd; } @@ -309,13 +338,14 @@ void* loop_GNSS_NMEA_device(void* dev) int maxfd; /* maximum file desciptor used */ int linecount=0; char buf[255]; + NMEA_RESULT trigger = NMEA_GPRMC; //gps data as returned by NMEA parser GPS_DATA gps_data; HNMEA_Init_GPS_DATA(&gps_data); /* loop until we have a terminating condition */ - LOG_DEBUG(gContext, "entering NMEA reading loop %d\n", fd); + //LOG_DEBUG(gContext, "entering NMEA reading loop %d\n", fd); while (g_GNSS_NMEA_loop) { int res; @@ -330,11 +360,11 @@ void* loop_GNSS_NMEA_device(void* dev) res = select(maxfd, &readfs, NULL, NULL, &Timeout); if (res==-1) { - LOG_DEBUG_MSG(gContext, "select()\n"); + //LOG_DEBUG_MSG(gContext, "select()\n"); } else if (res==0) { - LOG_DEBUG_MSG(gContext, "TIMEOUT\n"); + //LOG_DEBUG_MSG(gContext, "TIMEOUT\n"); } else if (FD_ISSET(fd, &readfs)) { @@ -342,10 +372,24 @@ void* loop_GNSS_NMEA_device(void* dev) buf[res]=0; /* set end of string, so we can printf */ linecount++; //LOG_DEBUG(gContext, "%d:%s", linecount, buf); + //printf("%"PRIu64":%s",gnss_get_timestamp(), buf); NMEA_RESULT nmea_res = HNMEA_Parse(buf, &gps_data); - if (nmea_res == NMEA_GPRMC) + + //most receivers sent GPRMC as last, but u-blox send as first: use other trigger + //determine most suitable trigger on actually received messages + #ifdef GNSS_CHIPSET_UBLOX + if (nmea_res == NMEA_GPGST) //highest precedence + { + trigger = NMEA_GPGST; + } + if ((nmea_res == NMEA_GPGSA) && (trigger == NMEA_GPRMC)) //GSA better than RMC { - uint64_t timestamp = gnss_get_timestamp(); + trigger = NMEA_GPGSA; + } + #endif + if (nmea_res == trigger) + { + uint64_t timestamp = gnss_get_timestamp() - GNSS_DELAY; TGNSSTime gnss_time = { 0 }; TGNSSPosition gnss_pos = { 0 }; if (extractTime(gps_data, timestamp, gnss_time)) @@ -378,6 +422,13 @@ int g_fd = -1; extern bool gnssInit() { g_fd = open_GNSS_NMEA_device(GNSS_DEVICE, GNSS_BAUDRATE); + //U-blox receivers: try to activate GPGST +#ifdef GNSS_CHIPSET_UBLOX + char act_gst[] = "$PUBX,40,GST,0,0,0,1,0,0*5A\r\n"; + //printf("GNSS_CHIPSET == UBLOX\n"); + write(g_fd, act_gst, strlen(act_gst)); +#endif + if (g_fd >=0) { pthread_create (&g_thread, NULL, loop_GNSS_NMEA_device, &g_fd); @@ -385,7 +436,7 @@ extern bool gnssInit() } else { - perror(GNSS_DEVICE); + //perror(GNSS_DEVICE); return false; } } @@ -394,7 +445,7 @@ extern bool gnssDestroy() { g_GNSS_NMEA_loop = 0; pthread_join (g_thread, NULL); - LOG_DEBUG_MSG(gContext, "gnssDestroy: NMEA reader thread terminated\n"); + //LOG_DEBUG_MSG(gContext, "gnssDestroy: NMEA reader thread terminated\n"); return true; } diff --git a/gnss-service/src/hnmea.cpp b/gnss-service/src/hnmea.cpp index 1bafdf5..b059b70 100644 --- a/gnss-service/src/hnmea.cpp +++ b/gnss-service/src/hnmea.cpp @@ -22,6 +22,7 @@ #include "hnmea.h" #include "string.h" #include "stdlib.h" +#include "math.h" //some example/test strings char test_gprmc[] = "$GPRMC,112911.000,A,4856.3328,N,01146.8259,E,35.75,108.51,050807,,,A*57"; @@ -55,6 +56,9 @@ void HNMEA_Init_GPS_DATA(GPS_DATA* gps_data) gps_data->usat = -99; gps_data->fix2d = -1; gps_data->fix3d = -1; + gps_data->hacc = 999.9; + gps_data->vacc = 999.9; + } @@ -680,6 +684,143 @@ void HNMEA_Parse_GPGSA(char* line, GPS_DATA* gps_data) gps_data->valid |= gps_data->valid_new; } +void HNMEA_Parse_GPGST(char* line, GPS_DATA* gps_data) +{ + enum { MAX_FIELD_LEN = 128}; + char field[MAX_FIELD_LEN]; + int i = 0; // field index + int l = 0; // line character index + int f = 0; // field character index + int stop = 0; // stop flag + int len = strlen(line); + + + float lat_std = 0.0; + int lat_std_valid = 0; + float lon_std = 0.0; + float alt_std = 0.0; + + gps_data->valid_new = 0; + + //outer loop - stop at line end + while ( (l < len ) && (stop == 0) ) + { + //inner loop - stop at line and and field separator + while ( (f < MAX_FIELD_LEN) && (l< len) && (line[l] != ',') && (line[l] != '*') ) + { + field[f] = line[l]; + l++; + f++; + } + field[f] = '\0'; // add string terminator + + switch (i) + { + case 0: //$GPGST + { + //cross-check for sentence name + if (strncmp (field, "$GPGST", 6) != 0) + { + // force termination of loop + stop = 1; + } + break; + } + case 1: //time hhmmss.sss + { + //length check + if (strlen (field) >=6) + { + gps_data->time_ss = atoi(field+4); + field[4] = '\0'; + gps_data->time_mm = atoi(field+2); + field[2] = '\0'; + gps_data->time_hh = atoi(field); + gps_data->valid_new |= GPS_DATA_TIME; + } + break; + } + case 2: // RMS value of the standard deviation of the ranges + { + //ignore + break; + } + case 3: //Standard deviation of semi-major axis, + { + //ignore + break; + } + case 4: //Standard deviation of semi-minor axis + { + //ignore + break; + } + case 5: //Orientation of semi-major axis + { + //ignore + break; + } + case 6: //Standard deviation of latitude, error in meters + { + //length check + if (strlen (field) >=1) + { + lat_std = atof(field); + lat_std_valid = 1; + } + break; + } + case 7: //Standard deviation of longitude, error in meters + { + //length check + if ((strlen (field) >=1) && (lat_std_valid)) + { + lon_std = atof(field); + gps_data->hacc = sqrt(lat_std*lat_std + lon_std*lon_std); + gps_data->valid_new |= GPS_DATA_HACC; + } + break; + } + case 8: //Standard deviation of altitude, error in meters + { + //length check + if (strlen (field) >=1) + { + alt_std = atof(field); + gps_data->vacc = alt_std; + gps_data->valid_new |= GPS_DATA_VACC; + } + break; + } + default: + { + stop = 1; + break; + } + } + + //one more field? + if ( line[l] == ',' ) + { + //skip separator + l++; + //reset + f = 0; + //increment field index + i++; + } + else + { + // force termination of loop + stop = 1; + } + } + + //update validity mask with new data + gps_data->valid |= gps_data->valid_new; +} + + NMEA_RESULT HNMEA_Parse(char* line, GPS_DATA* gps_data) { @@ -720,5 +861,17 @@ NMEA_RESULT HNMEA_Parse(char* line, GPS_DATA* gps_data) ret = NMEA_BAD_CHKSUM; } } + if (strncmp (line, "$GPGST", 6) == 0) + { + if (HNMEA_Checksum_Valid(line)) + { + HNMEA_Parse_GPGST(line, gps_data); + ret = NMEA_GPGST; + } + else + { + ret = NMEA_BAD_CHKSUM; + } + } return ret; } diff --git a/gnss-service/src/hnmea.h b/gnss-service/src/hnmea.h index 5dab01c..b26184d 100644 --- a/gnss-service/src/hnmea.h +++ b/gnss-service/src/hnmea.h @@ -32,7 +32,8 @@ typedef enum { NMEA_GPRMC, //GPRMC Sentence NMEA_GPGGA, //GPGGA Sentence NMEA_GPGSA, //GPGSA Sentence - NMEA_GPGSV //GPGSV Sentence + NMEA_GPGSV, //GPGSV Sentence + NMEA_GPGST //GPGST Sentence } NMEA_RESULT; //bitmap for GPS data @@ -50,7 +51,9 @@ typedef enum { GPS_DATA_PDOP = 0x0400, //PDOP GPS_DATA_USAT = 0x0800, //number of used satellites GPS_DATA_FIX2D = 0x1000, //at least 2D Fix - GPS_DATA_FIX3D = 0x2000 //3D Fix (GPS_DATA_FIX2D will be set also) + GPS_DATA_FIX3D = 0x2000, //3D Fix (GPS_DATA_FIX2D will be set also) + GPS_DATA_HACC = 0x4000, //horizontal accuracy + GPS_DATA_VACC = 0x8000 //vertical accuracy } GPS_DATA_TYPE; typedef struct { @@ -74,6 +77,8 @@ typedef struct { int usat; //number of satellites int fix2d; //GPS status: at least 2D Fix int fix3d; //GPS status: 3D Fix - fix2d will be set also + float hacc; //horizontal accuracy in m + float vacc; //vertical accuracy in m } GPS_DATA; void HNMEA_Init_GPS_DATA(GPS_DATA* gps_data); diff --git a/gnss-service/test/gnss-service-client.c b/gnss-service/test/gnss-service-client.c index 85d0472..ad76b32 100644 --- a/gnss-service/test/gnss-service-client.c +++ b/gnss-service/test/gnss-service-client.c @@ -74,7 +74,7 @@ static void cbPosition(const TGNSSPosition position[], uint16_t numElements) for (i = 0; i<numElements; i++) { - LOG_INFO(gCtx,"Position Update[%d/%d]: timestamp=%llu latitude=%.5f longitude=%.5f altitudeMSL=%.1f hSpeed=%.1f heading=%.1f\n hdop=%.1f usedSatellites=%d sigmaHPosition=%.1f sigmaHSpeed=%.1f sigmaHeading=%.1f\n fixStatus=%d fixTypeBits=0x%08X activated_systems=0x%08X used_systems=0x%08X", + LOG_INFO(gCtx,"Position Update[%d/%d]: timestamp=%llu latitude=%.5f longitude=%.5f altitudeMSL=%.1f hSpeed=%.1f heading=%.1f\n hdop=%.1f usedSatellites=%d sigmaHPosition=%.1f sigmaHSpeed=%.1f sigmaHeading=%.1f\n fixStatus=%d fixTypeBits=0x%08X activatedSystems=0x%08X usedSystems=0x%08X", i+1, numElements, position[i].timestamp, diff --git a/logger/CMakeLists.txt b/logger/CMakeLists.txt new file mode 100644 index 0000000..2140a96 --- /dev/null +++ b/logger/CMakeLists.txt @@ -0,0 +1,50 @@ +########################################################################### +# @licence app begin@ +# SPDX-License-Identifier: MPL-2.0 +# +# Component Name: Logger +# +# Author: 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/. +# +# Update (2015/07/10) : Helmut Schmidt <https://github.com/huirad>, +# - first version derived from log-replayer CMakeLists.txt +# @licence end@ +########################################################################### + +project(logger) +cmake_minimum_required(VERSION 2.6.0) + +option(WITH_DLT + "Enable the build to enable DLT logging " OFF) + +option(WITH_TESTS + "Compile test applications" OFF) + +option(WITH_GPSD + "Use GPSD as source of GPS data" OFF) + +option(WITH_NMEA + "Use NMEA as source of GPS data" OFF) + +option(WITH_MPU6050 + "Use MPU6050 as source of gyro/acceleration data" OFF) + +option(WITH_REPLAYER + "Use REPLAYER as source of GPS data" ON) + + + +message(STATUS "---------------------------------------------------------") + +add_subdirectory(src) +message(STATUS "---------------------------------------------------------") + +if(WITH_TESTS) + add_subdirectory(test) + message(STATUS "---------------------------------------------------------") +endif() diff --git a/logger/FindDLT.cmake b/logger/FindDLT.cmake new file mode 100644 index 0000000..c92aba9 --- /dev/null +++ b/logger/FindDLT.cmake @@ -0,0 +1,30 @@ +########################################################################### +# @licence app begin@ +# SPDX-License-Identifier: MPL-2.0 +# +# Component Name: LogReplayer +# +# Author: Marco Residori +# +# Copyright (C) 2014, XS Embedded GmbH +# +# License: +# This Source Code Form is subject to the terms of the +# Mozilla Public License, v. 2.0. If a copy of the MPL was not distributed with +# this file, You can obtain one at http://mozilla.org/MPL/2.0/. +# +# @licence end@ +########################################################################### + +set(DLT_INCLUDE_DIRS /usr/include/dlt) +set(DLT_LIBRARIES dlt) +set(DLT_LIBRARY_DIRS /usr/lib) + +find_package(PkgConfig) +pkg_check_modules(DLT REQUIRED automotive-dlt) + +if(${DLT_FOUND}) + #message(STATUS "found and use automotive-dlt: version ${DLT_VERSION}") +else() + message("missing DLT - check with 'pkg-config automotive-dlt --cflags-only-I'") +endif() diff --git a/logger/LICENSE b/logger/LICENSE new file mode 100644 index 0000000..14e2f77 --- /dev/null +++ b/logger/LICENSE @@ -0,0 +1,373 @@ +Mozilla Public License Version 2.0 +================================== + +1. Definitions +-------------- + +1.1. "Contributor" + means each individual or legal entity that creates, contributes to + the creation of, or owns Covered Software. + +1.2. "Contributor Version" + means the combination of the Contributions of others (if any) used + by a Contributor and that particular Contributor's Contribution. + +1.3. "Contribution" + means Covered Software of a particular Contributor. + +1.4. "Covered Software" + means Source Code Form to which the initial Contributor has attached + the notice in Exhibit A, the Executable Form of such Source Code + Form, and Modifications of such Source Code Form, in each case + including portions thereof. + +1.5. "Incompatible With Secondary Licenses" + means + + (a) that the initial Contributor has attached the notice described + in Exhibit B to the Covered Software; or + + (b) that the Covered Software was made available under the terms of + version 1.1 or earlier of the License, but not also under the + terms of a Secondary License. + +1.6. "Executable Form" + means any form of the work other than Source Code Form. + +1.7. "Larger Work" + means a work that combines Covered Software with other material, in + a separate file or files, that is not Covered Software. + +1.8. "License" + means this document. + +1.9. "Licensable" + means having the right to grant, to the maximum extent possible, + whether at the time of the initial grant or subsequently, any and + all of the rights conveyed by this License. + +1.10. "Modifications" + means any of the following: + + (a) any file in Source Code Form that results from an addition to, + deletion from, or modification of the contents of Covered + Software; or + + (b) any new file in Source Code Form that contains any Covered + Software. + +1.11. "Patent Claims" of a Contributor + means any patent claim(s), including without limitation, method, + process, and apparatus claims, in any patent Licensable by such + Contributor that would be infringed, but for the grant of the + License, by the making, using, selling, offering for sale, having + made, import, or transfer of either its Contributions or its + Contributor Version. + +1.12. "Secondary License" + means either the GNU General Public License, Version 2.0, the GNU + Lesser General Public License, Version 2.1, the GNU Affero General + Public License, Version 3.0, or any later versions of those + licenses. + +1.13. "Source Code Form" + means the form of the work preferred for making modifications. + +1.14. "You" (or "Your") + means an individual or a legal entity exercising rights under this + License. For legal entities, "You" includes any entity that + controls, is controlled by, or is under common control with You. For + purposes of this definition, "control" means (a) the power, direct + or indirect, to cause the direction or management of such entity, + whether by contract or otherwise, or (b) ownership of more than + fifty percent (50%) of the outstanding shares or beneficial + ownership of such entity. + +2. License Grants and Conditions +-------------------------------- + +2.1. Grants + +Each Contributor hereby grants You a world-wide, royalty-free, +non-exclusive license: + +(a) under intellectual property rights (other than patent or trademark) + Licensable by such Contributor to use, reproduce, make available, + modify, display, perform, distribute, and otherwise exploit its + Contributions, either on an unmodified basis, with Modifications, or + as part of a Larger Work; and + +(b) under Patent Claims of such Contributor to make, use, sell, offer + for sale, have made, import, and otherwise transfer either its + Contributions or its Contributor Version. + +2.2. Effective Date + +The licenses granted in Section 2.1 with respect to any Contribution +become effective for each Contribution on the date the Contributor first +distributes such Contribution. + +2.3. Limitations on Grant Scope + +The licenses granted in this Section 2 are the only rights granted under +this License. No additional rights or licenses will be implied from the +distribution or licensing of Covered Software under this License. +Notwithstanding Section 2.1(b) above, no patent license is granted by a +Contributor: + +(a) for any code that a Contributor has removed from Covered Software; + or + +(b) for infringements caused by: (i) Your and any other third party's + modifications of Covered Software, or (ii) the combination of its + Contributions with other software (except as part of its Contributor + Version); or + +(c) under Patent Claims infringed by Covered Software in the absence of + its Contributions. + +This License does not grant any rights in the trademarks, service marks, +or logos of any Contributor (except as may be necessary to comply with +the notice requirements in Section 3.4). + +2.4. Subsequent Licenses + +No Contributor makes additional grants as a result of Your choice to +distribute the Covered Software under a subsequent version of this +License (see Section 10.2) or under the terms of a Secondary License (if +permitted under the terms of Section 3.3). + +2.5. Representation + +Each Contributor represents that the Contributor believes its +Contributions are its original creation(s) or it has sufficient rights +to grant the rights to its Contributions conveyed by this License. + +2.6. Fair Use + +This License is not intended to limit any rights You have under +applicable copyright doctrines of fair use, fair dealing, or other +equivalents. + +2.7. Conditions + +Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted +in Section 2.1. + +3. Responsibilities +------------------- + +3.1. Distribution of Source Form + +All distribution of Covered Software in Source Code Form, including any +Modifications that You create or to which You contribute, must be under +the terms of this License. You must inform recipients that the Source +Code Form of the Covered Software is governed by the terms of this +License, and how they can obtain a copy of this License. You may not +attempt to alter or restrict the recipients' rights in the Source Code +Form. + +3.2. Distribution of Executable Form + +If You distribute Covered Software in Executable Form then: + +(a) such Covered Software must also be made available in Source Code + Form, as described in Section 3.1, and You must inform recipients of + the Executable Form how they can obtain a copy of such Source Code + Form by reasonable means in a timely manner, at a charge no more + than the cost of distribution to the recipient; and + +(b) You may distribute such Executable Form under the terms of this + License, or sublicense it under different terms, provided that the + license for the Executable Form does not attempt to limit or alter + the recipients' rights in the Source Code Form under this License. + +3.3. Distribution of a Larger Work + +You may create and distribute a Larger Work under terms of Your choice, +provided that You also comply with the requirements of this License for +the Covered Software. If the Larger Work is a combination of Covered +Software with a work governed by one or more Secondary Licenses, and the +Covered Software is not Incompatible With Secondary Licenses, this +License permits You to additionally distribute such Covered Software +under the terms of such Secondary License(s), so that the recipient of +the Larger Work may, at their option, further distribute the Covered +Software under the terms of either this License or such Secondary +License(s). + +3.4. Notices + +You may not remove or alter the substance of any license notices +(including copyright notices, patent notices, disclaimers of warranty, +or limitations of liability) contained within the Source Code Form of +the Covered Software, except that You may alter any license notices to +the extent required to remedy known factual inaccuracies. + +3.5. Application of Additional Terms + +You may choose to offer, and to charge a fee for, warranty, support, +indemnity or liability obligations to one or more recipients of Covered +Software. However, You may do so only on Your own behalf, and not on +behalf of any Contributor. You must make it absolutely clear that any +such warranty, support, indemnity, or liability obligation is offered by +You alone, and You hereby agree to indemnify every Contributor for any +liability incurred by such Contributor as a result of warranty, support, +indemnity or liability terms You offer. You may include additional +disclaimers of warranty and limitations of liability specific to any +jurisdiction. + +4. Inability to Comply Due to Statute or Regulation +--------------------------------------------------- + +If it is impossible for You to comply with any of the terms of this +License with respect to some or all of the Covered Software due to +statute, judicial order, or regulation then You must: (a) comply with +the terms of this License to the maximum extent possible; and (b) +describe the limitations and the code they affect. Such description must +be placed in a text file included with all distributions of the Covered +Software under this License. Except to the extent prohibited by statute +or regulation, such description must be sufficiently detailed for a +recipient of ordinary skill to be able to understand it. + +5. Termination +-------------- + +5.1. The rights granted under this License will terminate automatically +if You fail to comply with any of its terms. However, if You become +compliant, then the rights granted under this License from a particular +Contributor are reinstated (a) provisionally, unless and until such +Contributor explicitly and finally terminates Your grants, and (b) on an +ongoing basis, if such Contributor fails to notify You of the +non-compliance by some reasonable means prior to 60 days after You have +come back into compliance. Moreover, Your grants from a particular +Contributor are reinstated on an ongoing basis if such Contributor +notifies You of the non-compliance by some reasonable means, this is the +first time You have received notice of non-compliance with this License +from such Contributor, and You become compliant prior to 30 days after +Your receipt of the notice. + +5.2. If You initiate litigation against any entity by asserting a patent +infringement claim (excluding declaratory judgment actions, +counter-claims, and cross-claims) alleging that a Contributor Version +directly or indirectly infringes any patent, then the rights granted to +You by any and all Contributors for the Covered Software under Section +2.1 of this License shall terminate. + +5.3. In the event of termination under Sections 5.1 or 5.2 above, all +end user license agreements (excluding distributors and resellers) which +have been validly granted by You or Your distributors under this License +prior to termination shall survive termination. + +************************************************************************ +* * +* 6. Disclaimer of Warranty * +* ------------------------- * +* * +* Covered Software is provided under this License on an "as is" * +* basis, without warranty of any kind, either expressed, implied, or * +* statutory, including, without limitation, warranties that the * +* Covered Software is free of defects, merchantable, fit for a * +* particular purpose or non-infringing. The entire risk as to the * +* quality and performance of the Covered Software is with You. * +* Should any Covered Software prove defective in any respect, You * +* (not any Contributor) assume the cost of any necessary servicing, * +* repair, or correction. This disclaimer of warranty constitutes an * +* essential part of this License. No use of any Covered Software is * +* authorized under this License except under this disclaimer. * +* * +************************************************************************ + +************************************************************************ +* * +* 7. Limitation of Liability * +* -------------------------- * +* * +* Under no circumstances and under no legal theory, whether tort * +* (including negligence), contract, or otherwise, shall any * +* Contributor, or anyone who distributes Covered Software as * +* permitted above, be liable to You for any direct, indirect, * +* special, incidental, or consequential damages of any character * +* including, without limitation, damages for lost profits, loss of * +* goodwill, work stoppage, computer failure or malfunction, or any * +* and all other commercial damages or losses, even if such party * +* shall have been informed of the possibility of such damages. This * +* limitation of liability shall not apply to liability for death or * +* personal injury resulting from such party's negligence to the * +* extent applicable law prohibits such limitation. Some * +* jurisdictions do not allow the exclusion or limitation of * +* incidental or consequential damages, so this exclusion and * +* limitation may not apply to You. * +* * +************************************************************************ + +8. Litigation +------------- + +Any litigation relating to this License may be brought only in the +courts of a jurisdiction where the defendant maintains its principal +place of business and such litigation shall be governed by laws of that +jurisdiction, without reference to its conflict-of-law provisions. +Nothing in this Section shall prevent a party's ability to bring +cross-claims or counter-claims. + +9. Miscellaneous +---------------- + +This License represents the complete agreement concerning the subject +matter hereof. If any provision of this License is held to be +unenforceable, such provision shall be reformed only to the extent +necessary to make it enforceable. Any law or regulation which provides +that the language of a contract shall be construed against the drafter +shall not be used to construe this License against a Contributor. + +10. Versions of the License +--------------------------- + +10.1. New Versions + +Mozilla Foundation is the license steward. Except as provided in Section +10.3, no one other than the license steward has the right to modify or +publish new versions of this License. Each version will be given a +distinguishing version number. + +10.2. Effect of New Versions + +You may distribute the Covered Software under the terms of the version +of the License under which You originally received the Covered Software, +or under the terms of any subsequent version published by the license +steward. + +10.3. Modified Versions + +If you create software not governed by this License, and you want to +create a new license for such software, you may create and use a +modified version of this License if you rename the license and remove +any references to the name of the license steward (except to note that +such modified license differs from this License). + +10.4. Distributing Source Code Form that is Incompatible With Secondary +Licenses + +If You choose to distribute Source Code Form that is Incompatible With +Secondary Licenses under the terms of this version of the License, the +notice described in Exhibit B of this License must be attached. + +Exhibit A - Source Code Form License Notice +------------------------------------------- + + 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/. + +If it is not possible or desirable to put the notice in a particular +file, then You may include the notice in a location (such as a LICENSE +file in a relevant directory) where a recipient would be likely to look +for such a notice. + +You may add additional accurate notices of copyright ownership. + +Exhibit B - "Incompatible With Secondary Licenses" Notice +--------------------------------------------------------- + + This Source Code Form is "Incompatible With Secondary Licenses", as + defined by the Mozilla Public License, v. 2.0. diff --git a/logger/README b/logger/README new file mode 100644 index 0000000..5b4e553 --- /dev/null +++ b/logger/README @@ -0,0 +1,10 @@ +Positioning Logger +================================== +Author Helmut Schmidt <https://github.com/huirad> + +Overview +-------- +The positioning logger may be used to write data provided GNSSService and SensorsService to DLT, a file or another sink. +It is a tiny library without further dependencies which can be linked to each application. +It is agnostic of the data written to the log. +The positioning logger is not an official GENIVI component. diff --git a/logger/inc/poslog.h b/logger/inc/poslog.h new file mode 100644 index 0000000..f1dbbd8 --- /dev/null +++ b/logger/inc/poslog.h @@ -0,0 +1,185 @@ +/************************************************************************** +* @licence app begin@ +* +* SPDX-License-Identifier: MPL-2.0 +* +* \brief Logging service for data from positioning and similar sources +* Log data can be forwarded to different sinks in parallel, +* e.g. to DLT and to a file or a socket +* Additionally a custom callback can be registered as log sink +* Log data can be provided from different threads running parallel +* Logging is done synchronously, so it may block temporarily +* depending on the type of sinks which are active +* However using the callback sink, an asynchronous logging +* can be implemented without impact to the threads providing log data +* Log data must be provided as ASCII strings +* The clients are responsible for string formatting +* according the positioning log format specification +* +* +* \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@ +**************************************************************************/ + +#ifndef INCLUDE_GENIVI_POS_LOG +#define INCLUDE_GENIVI_POS_LOG + +#include <stdint.h> +#include <stdbool.h> + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * EPoslogReleaseLevel indicates the release level of this API + * This enum follows the release level convention used by python + * @ref https://docs.python.org/3/c-api/apiabiversion.html + */ +typedef enum { + POSLOG_REL_ALPHA = 0xA0, /**< API is in alpha state, i.e. work in progress. */ + POSLOG_REL_BETA = 0xB0, /**< API is in beta state, i.e. close to be finished. */ + POSLOG_REL_CANDIDATE = 0xC0, /**< API is in release candidate state. */ + POSLOG_REL_FINAL = 0xF0, /**< API is in final state, i.e. officially approved. */ +} EPoslogReleaseLevel; + + +// API Version +#define GENIVI_POSLOG_MAJOR 1 +#define GENIVI_POSLOG_MINOR 0 +#define GENIVI_POSLOG_MICRO 0 +#define GENIVI_POSLOG_LEVEL POSLOG_REL_ALPHA + + +/** + * TPoslogSinks is used to indicate which sinks are active. + * It is a or'ed bitmask of the EPoslogSinks values. + * Multiple sinks can be active in parallel + */ +typedef uint32_t TPoslogSinks; +typedef enum { + POSLOG_SINK_DLT = 0x00000001, /**< Bit is set to indicate logging to GENIVI DLT. */ + POSLOG_SINK_SYSLOG = 0x00000002, /**< Bit is set to indicate logging to Linux syslog. + Note: on most systemd based systems, syslog is not running. + But on systems without systemd, this might be still useful. */ + POSLOG_SINK_FD = 0x00000004, /**< Bit is set to indicate logging to a file descriptor. + The file descriptor can e.g. refer to a file, pipe, socket, ... . + The file descriptor must be registered using poslogSetFD(). */ + POSLOG_SINK_CB = 0x00000008, /**< Bit is set to indicate logging to custom callback function. + The callback function must be registered using poslogSetCB(). */ +} EPoslogSinks; + +/** + * TPoslogSeq is used to allow uninterrupted logging of a sequence of strings + * It is a or'ed bitmask of the EPoslogSeq values. + */ +typedef uint16_t TPoslogSeq; +typedef enum { + POSLOG_SEQ_CONT = 0x0000, /**< String to be logged is in the middle of a sequence. */ + POSLOG_SEQ_START = 0x0001, /**< String to be logged is at the start of a sequence. */ + POSLOG_SEQ_STOP = 0x0002, /**< String to be logged is at the end of a sequence. */ + POSLOG_SEQ_SINGLE = 0x0003, /**< String to be logged is not part of a sequence. */ +} EPoslogSeq; + + +/** + * Callback type for a custom log sink provided by the application. + * Use this type of callback if you want to send log data + * to sinks which are not directly supported by the positioning logger. + * @param logstring String to be added to the log. + */ +typedef void (*PoslogCallback)(const char* string); + + +/** + * Initialization of the positioning logging service. + * Must be called before using the positioning logging service to set up the service. + * If the DLT sink shall be used, the caller is responsible to call DLT_REGISTER_APP() + * before poslogInit(). + * If the syslog sink shall be used, the caller is responsible to call openlog() + * before poslogInit(). + * @return True if initialization has been successfull. + */ +bool poslogInit(); + +/** + * Destroy the positioning logging service. + * Must be called after using the positioning logging service to shut down the service. + * @return True if shutdown has been successfull. + */ +bool poslogDestroy(); + +/** + * Positioning logging services version information. + * @param major Major version number. Changes in this number are used for incompatible API change. + * @param minor Minor version number. Changes in this number are used for compatible API change. + * @param micro Micro version number. Changes in this number are used for minor changes. + * @param level Release level of this API + */ +void poslogGetVersion(int *major, int *minor, int *micro, EPoslogReleaseLevel *level); + +/** + * Control which sinks are active + * @param sinks An or'ed bitmask of the EPoslogSinks values. + * A sink will be activated when the corresponding bit is set. + * For file descriptor or callback sinks it is advised to set the + * corresponding file descriptor or callback before, otherwise logs may be lost. + * This function is thread safe and can be called during logging + * @return void + */ +void poslogSetActiveSinks(TPoslogSinks sinks); + +/** + * Determine which sinks are active + * @return sinks An or'ed bitmask of the EPoslogSinks values. + * A sink will is activate when the corresponding bit is set. + */ +TPoslogSinks poslogGetActiveSinks(); + +/** + * Set the file descriptor for the file descriptor sink + * Only one file descriptor can be used at a time + * The file descriptor must refer to an *open* file/socket/pipe... + * This function is thread safe and can be called during logging + * Calling this function will not automatically activate the fd sink, + * use @ref poslogSetActiveSinks() for this. + * @param fd The file descriptor to be used as logging sink + * @return The previously set file descriptor, -1 if no file descriptor was set + */ +int poslogSetFD(int fd); + +/** + * Set the file descriptor for the file descriptor sink + * Only one file descriptor can be used at a time + * This function is thread safe and can be called during logging. + * Calling this function will not automatically activate the fd sink, + * use @ref poslogSetActiveSinks() for this. + * @param cb The callback to be used as logging sink + * @return The previously set callback, NULL if no callback was set + */ +PoslogCallback poslogSetCB(PoslogCallback cb); + +/** + * Add a string to the log. + * The string will be sent to all currently active sinks. + * This function is thread safe. Log strings can be provided from concurrent threads. + * Note: The log string shall *not* contain a trailing newline. + * @note Depending on the type of sink there might be length limitations. + * @param logstring String to be added to the log. + * @param seq Bitmask of the EPoslogSeq values indicating where in a sequence the string is. + */ +void poslogAddString(const char* logstring, TPoslogSeq seq = POSLOG_SEQ_SINGLE); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/logger/src/CMakeLists.txt b/logger/src/CMakeLists.txt new file mode 100644 index 0000000..7471a68 --- /dev/null +++ b/logger/src/CMakeLists.txt @@ -0,0 +1,47 @@ +########################################################################### +# @licence app begin@ +# SPDX-License-Identifier: MPL-2.0 +# +# Component Name: Logger +# +# Author: 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/. +# +# Update (2015/07/10) : Helmut Schmidt <https://github.com/huirad>, +# - first version derived from log-replayer CMakeLists.txt +# +# @licence end@ +########################################################################### + +message(STATUS "LOGGER") +message(STATUS "WITH_DLT = ${WITH_DLT}") +message(STATUS "WITH_TESTS = ${WITH_TESTS}") + +include_directories("${PROJECT_SOURCE_DIR}/inc") + +find_package(PkgConfig) + +set(LIB_SRCS ${CMAKE_CURRENT_SOURCE_DIR}/poslog.cpp) + +set(LIBRARIES pthread) + +if(WITH_DLT) + set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}") + find_package(DLT REQUIRED) + add_definitions("-DDLT_ENABLED=1") + include_directories( ${DLT_INCLUDE_DIRS} ) + set(LIBRARIES ${LIBRARIES} ${DLT_LIBRARIES}) +endif() + +set(LIB_SRC_LOGGER ${CMAKE_CURRENT_SOURCE_DIR}/poslog.cpp) +add_library(poslog SHARED ${LIB_SRC_LOGGER}) +install(TARGETS poslog DESTINATION lib) + +message(STATUS "/logger------------------------------------------------") + + + diff --git a/logger/src/poslog.cpp b/logger/src/poslog.cpp new file mode 100644 index 0000000..b2dbc12 --- /dev/null +++ b/logger/src/poslog.cpp @@ -0,0 +1,155 @@ +/************************************************************************** +* @licence app begin@ +* +* SPDX-License-Identifier: MPL-2.0 +* +* \brief Logging service for data from positioning and similar sources +* Log data can be forwarded to different sinks in parallel, +* e.g. to DLT and to a file or a socket +* Additionally a custom callback can be registered as log sink +* Log data can be provided from different threads running parallel +* Logging is done synchronously, so it may block temporarily +* depending on the type of sinks which are active +* However using the callback sink, an asynchronous logging +* can be implemented without impact to the threads providing log data +* Log data must be provided as ASCII strings +* The clients are responsible for string formatting +* according the positioning log format specification +* +* +* \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@ +**************************************************************************/ + +#include "poslog.h" +#if (DLT_ENABLED) +#include "dlt.h" +#endif +#include <syslog.h> +#include <unistd.h> +#include <string.h> +#include <stdio.h> +#include <pthread.h> + +static pthread_mutex_t mutexLog = PTHREAD_MUTEX_INITIALIZER; //protects everything +static TPoslogSinks g_active_sinks = 0; +static int g_fd = -1; +static PoslogCallback g_callback = NULL; +#if (DLT_ENABLED) +DLT_DECLARE_CONTEXT(poslogContext); +#endif + +bool poslogInit() +{ + pthread_mutex_lock(&mutexLog); + g_active_sinks = 0; + int g_fd = -1; + g_callback= NULL; +#if (DLT_ENABLED) + DLT_REGISTER_CONTEXT(poslogContext,"POSL","Positioning Logging"); +#endif + pthread_mutex_unlock(&mutexLog); + return true; +} + +bool poslogDestroy() +{ + pthread_mutex_lock(&mutexLog); + g_active_sinks = 0; + int g_fd = -1; + g_callback= NULL; +#if (DLT_ENABLED) + DLT_UNREGISTER_CONTEXT(poslogContext); +#endif + pthread_mutex_unlock(&mutexLog); + return true; +} + +void poslogGetVersion(int *major, int *minor, int *micro, EPoslogReleaseLevel *level) +{ + *major = GENIVI_POSLOG_MAJOR; + *minor = GENIVI_POSLOG_MINOR; + *micro = GENIVI_POSLOG_MICRO; + *level = GENIVI_POSLOG_LEVEL; +} + +void poslogSetActiveSinks(TPoslogSinks sinks) +{ + pthread_mutex_lock(&mutexLog); + g_active_sinks = sinks; + pthread_mutex_unlock(&mutexLog); +} + +TPoslogSinks poslogGetActiveSinks() +{ + pthread_mutex_lock(&mutexLog); + TPoslogSinks sinks = g_active_sinks; + pthread_mutex_unlock(&mutexLog); + return sinks; +} + +int poslogSetFD(int fd) +{ + pthread_mutex_lock(&mutexLog); + int old_fd = g_fd; + g_fd = fd; + pthread_mutex_unlock(&mutexLog); + return old_fd; +} + +PoslogCallback poslogSetCB(PoslogCallback cb) +{ + pthread_mutex_lock(&mutexLog); + PoslogCallback old_callback = g_callback; + g_callback = cb; + pthread_mutex_unlock(&mutexLog); + return old_callback; +} + +static void poslogAddString_nolock(const char* logstring) +{ +#if (DLT_ENABLED) + if (g_active_sinks & POSLOG_SINK_DLT) + { + DLT_LOG(poslogContext, DLT_LOG_INFO, DLT_STRING(logstring)); + } +#endif + if (g_active_sinks & POSLOG_SINK_SYSLOG) + { + //syslog(LOG_INFO, logstring); + syslog(LOG_EMERG, logstring); + } + if (g_active_sinks & POSLOG_SINK_FD) + { + write(g_fd, logstring, strlen(logstring)); + write(g_fd, "\n", 1); + } + if (g_active_sinks & POSLOG_SINK_CB) + { + if (g_callback) g_callback(logstring); + } +} + +void poslogAddString(const char* logstring, TPoslogSeq seq) +{ + if (seq & POSLOG_SEQ_START) + { + pthread_mutex_lock(&mutexLog); + } + poslogAddString_nolock(logstring); + if (seq & POSLOG_SEQ_STOP) + { + pthread_mutex_unlock(&mutexLog); + } +} + + + diff --git a/logger/test/CMakeLists.txt b/logger/test/CMakeLists.txt new file mode 100644 index 0000000..696a54f --- /dev/null +++ b/logger/test/CMakeLists.txt @@ -0,0 +1,115 @@ +########################################################################### +# @licence app begin@ +# SPDX-License-Identifier: MPL-2.0 +# +# Component Name: Logger +# +# Author: 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/. +# +# Update (2015/07/10) : Helmut Schmidt <https://github.com/huirad>, +# - first version derived from log-replayer CMakeLists.txt +# +# @licence end@ +########################################################################### + + +message(STATUS "TEST-LOGGER") +message(STATUS "WITH_DLT = ${WITH_DLT}") +message(STATUS "WITH_GPSD = ${WITH_GPSD}") +message(STATUS "WITH_NMEA = ${WITH_NMEA}") +message(STATUS "WITH_MPU6050 = ${WITH_MPU6050}") +message(STATUS "WITH_REPLAYER = ${WITH_REPLAYER}") +message(STATUS "WITH_DEBUG = ${WITH_DEBUG}") + +include_directories("${PROJECT_SOURCE_DIR}/inc") + +find_package(PkgConfig) + +set(SRCS ${CMAKE_CURRENT_SOURCE_DIR}/test-poslog.cpp) +add_executable(test-poslog ${SRCS}) +set(LIBRARIES pthread poslog) + + +if(WITH_DLT) + set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}") + find_package(DLT REQUIRED) + add_definitions("-DDLT_ENABLED=1") + include_directories( ${DLT_INCLUDE_DIRS} ) + set(LIBRARIES ${LIBRARIES} ${DLT_LIBRARIES}) +endif() + +if(WITH_DEBUG) + add_definitions("-DDEBUG_ENABLED=1") +endif() + +target_link_libraries(test-poslog ${LIBRARIES}) + +install(TARGETS test-poslog DESTINATION bin) + + +set(SRCS ${CMAKE_CURRENT_SOURCE_DIR}/log-gnss-sns.cpp +${CMAKE_CURRENT_SOURCE_DIR}/gnsslog.cpp +${CMAKE_CURRENT_SOURCE_DIR}/snslog.cpp) +add_executable(log-gnss-sns ${SRCS}) +set(LIBRARIES pthread poslog) + + +set(gnss-service_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/../gnss-service/api") +set(gnss-service_LIBRARY_DIRS "${PROJECT_BINARY_DIR}/../gnss-service/src") +if(WITH_GPSD) + set(GNSS_LIBRARIES "gnss-service-use-gpsd") +elseif(WITH_NMEA) + set(GNSS_LIBRARIES "gnss-service-use-nmea") +elseif(WITH_REPLAYER) + set(GNSS_LIBRARIES "gnss-service-use-replayer") +else() + message(STATUS "Invalid cmake options!") +endif() +message(STATUS "GNSS_LIBRARIES = ${GNSS_LIBRARIES}") +set(LIBRARIES ${LIBRARIES} ${GNSS_LIBRARIES}) + +set(sns-service_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/../sensors-service/api") +set(sns-service_LIBRARY_DIRS "${PROJECT_BINARY_DIR}/../sensors-service/src") +if(WITH_IPHONE) + set(SNS_LIBRARIES "sensors-service-use-iphone") +elseif(WITH_MPU6050) + set(SNS_LIBRARIES "sensors-service-use-mpu6050") +elseif(WITH_REPLAYER) + set(SNS_LIBRARIES "sensors-service-use-replayer") +else() + message(STATUS "Invalid cmake options!") +endif() +message(STATUS "SNS_LIBRARIES = ${SNS_LIBRARIES}") +set(LIBRARIES ${LIBRARIES} ${SNS_LIBRARIES}) +#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) + + +include_directories( + ${gnss-service_INCLUDE_DIRS} + ${sns-service_INCLUDE_DIRS} +) + +if(WITH_DLT) + set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}") + find_package(DLT REQUIRED) + add_definitions("-DDLT_ENABLED=1") + include_directories( ${DLT_INCLUDE_DIRS} ) + set(LIBRARIES ${LIBRARIES} ${DLT_LIBRARIES}) +endif() + +if(WITH_DEBUG) + add_definitions("-DDEBUG_ENABLED=1") +endif() + +target_link_libraries(log-gnss-sns ${LIBRARIES}) +#message(STATUS "LIBRARIES = ${LIBRARIES}") + +install(TARGETS log-gnss-sns DESTINATION bin) + diff --git a/logger/test/gnsslog.cpp b/logger/test/gnsslog.cpp new file mode 100644 index 0000000..e656507 --- /dev/null +++ b/logger/test/gnsslog.cpp @@ -0,0 +1,175 @@ +/************************************************************************** +* @licence app begin@ +* +* SPDX-License-Identifier: MPL-2.0 +* +* \brief Utility functions to create and log GNSS specific log strings +* +* +* \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@ +**************************************************************************/ + +#define __STDC_FORMAT_MACROS +#include <inttypes.h> + +#include "gnsslog.h" +#include "poslog.h" + +#include "gnss.h" + +#include <string.h> +#include <stdio.h> +#include <unistd.h> +#include <time.h> + + + +#define LOG_STRING_SIZE 256 + +uint64_t gnsslogGetTimestamp() +{ + 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; + } +} + +void gnssPositionToString(uint64_t timestamp, uint16_t countdown, const TGNSSPosition* position, char *str, size_t size) +{ + if ((str) && (size > 0)) + { + snprintf( + str, + size-1, //ensure that there is space for null-terminator + "%"PRIu64",%"PRIu16",$GVGNSPOS,%"PRIu64",%9.6f,%9.6f,%6.1f,%6.1f,%4.1f,%4.1f,%6.2f,%3.1f,%3.1f,%3.1f,%02"PRIu16",%02"PRIu16",%02"PRIu16",%4.1f,%4.1f,%4.1f,%4.1f,%4.1f,%u,0X%08X,0X%08X,0X%08X,0X%08X", + timestamp, + countdown, + position->timestamp, + position->latitude, + position->longitude, + position->altitudeMSL, + position->altitudeEll, + position->hSpeed, + position->vSpeed, + position->heading, + position->pdop, + position->hdop, + position->vdop, + position->usedSatellites, + position->trackedSatellites, + position->visibleSatellites, + position->sigmaHPosition, + position->sigmaAltitude, + position->sigmaHSpeed, + position->sigmaVSpeed, + position->sigmaHeading, + position->fixStatus, + position->fixTypeBits, + position->activatedSystems, + position->usedSystems, + position->validityBits + ); + str[size-1] = 0; //ensure that string is null-terminated + } +} + +void gnssTimeToString(uint64_t timestamp, uint16_t countdown, const TGNSSTime* time, char *str, size_t size) +{ + if ((str) && (size > 0)) + { + snprintf( + str, + size-1, //ensure that there is space for null-terminator + "%"PRIu64",%"PRIu16",$GVGNSTIM,%"PRIu64",%04"PRIu16",%02"PRIu8",%02"PRIu8",%02"PRIu8",%02"PRIu8",%02"PRIu8",%03"PRIu16",0X%08X", + timestamp, + countdown, + time->timestamp, + time->year, + time->month, + time->day, + time->hour, + time->minute, + time->second, + time->ms, + time->validityBits + ); + str[size-1] = 0; //ensure that string is null-terminated + } +} + +void gnssSatelliteDetailToString(uint64_t timestamp, uint16_t countdown, const TGNSSSatelliteDetail* satelliteDetails, char *str, size_t size) +{ + if ((str) && (size > 0)) + { + snprintf( + str, + size-1, //ensure that there is space for null-terminator + "%"PRIu64",%"PRIu16",$GVGNSSAT,%"PRIu64",%u,%"PRIu16",%"PRIu16",%"PRIu16",%"PRIu16",0X%08X,%"PRIu16",0X%08X", + timestamp, + countdown, + satelliteDetails->timestamp, + satelliteDetails->system, + satelliteDetails->satelliteId, + satelliteDetails->azimuth, + satelliteDetails->elevation, + satelliteDetails->SNR, + satelliteDetails->statusBits, + satelliteDetails->posResidual, + satelliteDetails->validityBits + ); + str[size-1] = 0; //ensure that string is null-terminated + } +} + +void gnssPositionLog(uint64_t timestamp, const TGNSSPosition position[], uint16_t numElements) +{ + char logstring[LOG_STRING_SIZE] ; + for (int i=0; i<numElements; i++) + { + TPoslogSeq seq = POSLOG_SEQ_CONT; + if (i==0) seq|=POSLOG_SEQ_START; + if (i==(numElements-1)) seq|=POSLOG_SEQ_STOP; + gnssPositionToString(timestamp, numElements-i-1, &position[i], logstring, LOG_STRING_SIZE); + poslogAddString(logstring, seq); + } +} + +void gnssTimeLog(uint64_t timestamp, const TGNSSTime time[], uint16_t numElements) +{ + char logstring[LOG_STRING_SIZE] ; + for (int i=0; i<numElements; i++) + { + TPoslogSeq seq = POSLOG_SEQ_CONT; + if (i==0) seq|=POSLOG_SEQ_START; + if (i==(numElements-1)) seq|=POSLOG_SEQ_STOP; + gnssTimeToString(timestamp, numElements-i-1, &time[i], logstring, LOG_STRING_SIZE); + poslogAddString(logstring, seq); + } +} + +void gnssSatelliteDetailLog(uint64_t timestamp, const TGNSSSatelliteDetail satelliteDetail[], uint16_t numElements) +{ + char logstring[LOG_STRING_SIZE] ; + for (int i=0; i<numElements; i++) + { + TPoslogSeq seq = POSLOG_SEQ_CONT; + if (i==0) seq|=POSLOG_SEQ_START; + if (i==(numElements-1)) seq|=POSLOG_SEQ_STOP; + gnssSatelliteDetailToString(timestamp, numElements-i-1, &satelliteDetail[i], logstring, LOG_STRING_SIZE); + poslogAddString(logstring, seq); + } +} diff --git a/logger/test/gnsslog.h b/logger/test/gnsslog.h new file mode 100644 index 0000000..634fbb8 --- /dev/null +++ b/logger/test/gnsslog.h @@ -0,0 +1,108 @@ +/************************************************************************** +* @licence app begin@ +* +* SPDX-License-Identifier: MPL-2.0 +* +* \brief Utility functions to create and log GNSS specific log strings +* +* +* \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@ +**************************************************************************/ + +#ifndef INCLUDE_GENIVI_GNSS_LOG +#define INCLUDE_GENIVI_GNSS_LOG + +#include "poslog.h" +#include "gnss.h" +#include <string.h> + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Provide a system timestamp in milliseconds. + * @return system timestamp in milliseconds + */ +uint64_t gnsslogGetTimestamp(); + +/** + * Convert a TGNSSPosition structure to a log string. + * @note: The log string will *not* contain a line break (\n) at the end. + * @param timestamp Timestamp for the current time in ms to be added to the header of the log string. + * This timestamp shall be based on the same time source as the timestamp within the position parameter. + * @param countdown Countdown value to be added to the header of the log string. + * This is the number of following GNSS position data in same sequence + * @param position TGNSSPosition structure to be converted to the log string + * @param str Pointer to a string variable where the log string will be written to. + * @param size Size of the string variable where the log string will be written to. + */ +void gnssPositionToString(uint64_t timestamp, uint16_t countdown, const TGNSSPosition* position, char *str, size_t size); + +/** + * Convert a TGNSSTime structure to a log string. + * @note: The log string will *not* contain a line break (\n) at the end. + * @param timestamp Timestamp for the current time in ms to be added to the header of the log string. + * This timestamp shall be based on the same time source as the timestamp within the time parameter. + * @param countdown Countdown value to be added to the header of the log string. + * This is the number of following GNSS time data in same sequence + * @param time TGNSSTime structure to be converted to the log string + * @param str Pointer to a string variable where the log string will be written to. + * @param size Size of the string variable where the log string will be written to. + */ +void gnssTimeToString(uint64_t timestamp, uint16_t countdown, const TGNSSTime* time, char *str, size_t size); + +/** + * Convert a TGNSSSatelliteDetail structure to a log string. + * @note: The log string will *not* contain a line break (\n) at the end. + * @param timestamp Timestamp for the current time in ms to be added to the header of the log string. + * This timestamp shall be based on the same time source as the timestamp within the satellite detail parameter. + * @param countdown Countdown value to be added to the header of the log string. + * This is the number of following GNSS satellite data in same sequence + * @param satelliteDetails TGNSSSatelliteDetail structure to be converted to the log string + * @param str Pointer to a string variable where the log string will be written to. + * @param size Size of the string variable where the log string will be written to. + */ +void gnssSatelliteDetailToString(uint64_t timestamp, uint16_t countdown, const TGNSSSatelliteDetail* satelliteDetails, char *str, size_t size); + +/** + * Write GNSS position data to the position log. + * + * @param timestamp Timestamp when the GNSS position data have been received [ms] + * @param position Pointer to an array of TGNSSPosition with size numElements + * @param numElements Number of TGNSSPosition elements in array position + */ +void gnssPositionLog(uint64_t timestamp, const TGNSSPosition position[], uint16_t numElements); + +/** + * Write GNSS time data to the position log. + * + * @param timestamp Timestamp when the GNSS time data have been received [ms] + * @param time Pointer to an array of TGNSSTime with size numElements + * @param numElements: Number of TGNSSTime elements in array time. + */ +void gnssTimeLog(uint64_t timestamp, const TGNSSTime time[], uint16_t numElements); + +/** + * Write GNSS satellite detail data to the position log. + * + * @param timestamp Timestamp when the GNSS satellite detail data have been received [ms] + * @param satelliteDetail Pointer to an array of TGNSSSatelliteDetail with size numElements + * @param numElements: Number of TGNSSSatelliteDetail elements in array satelliteDetail. + */ +void gnssSatelliteDetailLog(uint64_t timestamp, const TGNSSSatelliteDetail satelliteDetail[], uint16_t numElements); + +#ifdef __cplusplus +} +#endif + +#endif
\ No newline at end of file diff --git a/logger/test/log-gnss-sns.cpp b/logger/test/log-gnss-sns.cpp new file mode 100644 index 0000000..7e5c29c --- /dev/null +++ b/logger/test/log-gnss-sns.cpp @@ -0,0 +1,220 @@ +/************************************************************************** +* @licence app begin@ +* +* SPDX-License-Identifier: MPL-2.0 +* +* \brief Test program for GNSS logging +* +* +* \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@ +**************************************************************************/ + +#include "poslog.h" +#include "gnsslog.h" +#include "snslog.h" +#if (DLT_ENABLED) +#include "dlt.h" +#endif + +#include <syslog.h> +#include <unistd.h> +#include <string.h> +#include <stdio.h> +#include <pthread.h> +#include <signal.h> + +#include "gnss-init.h" +#include "gnss.h" +#include "sns-init.h" + +#define GNSS_INIT_MAX_RETRIES 30 + +//global variable to control the main loop - will be set by signal handlers +static volatile bool sigint = false; +static volatile bool sigterm = false; + +static void sigHandler (int sig, siginfo_t *siginfo, void *context) +{ + if (sig == SIGINT) + { + sigint = true; + printf("SIGINT\n"); + } + else + if (sig == SIGTERM) + { + sigterm = true; + } +} + +static bool registerSigHandlers() +{ + bool is_success = true; + + struct sigaction action; + memset (&action, '\0', sizeof(action)); + action.sa_sigaction = &sigHandler; + action.sa_flags = SA_SIGINFO; + + if (sigaction(SIGINT, &action, NULL) < 0) + { + is_success = false; + } + if (sigaction(SIGTERM, &action, NULL) < 0) + { + is_success = false; + } + return is_success; +} + + + +static void cbTime(const TGNSSTime time[], uint16_t numElements) +{ + gnssTimeLog(gnsslogGetTimestamp(), time, numElements); +} + +static void cbPosition(const TGNSSPosition position[], uint16_t numElements) +{ + gnssPositionLog(gnsslogGetTimestamp(), position, numElements); +} + +static void cbAccel(const TAccelerationData accelerationData[], uint16_t numElements) +{ + accelerationDataLog(snslogGetTimestamp(), accelerationData, numElements); +} + +static void cbGyro(const TGyroscopeData gyroData[], uint16_t numElements) +{ + gyroscopeDataLog(snslogGetTimestamp(), gyroData, numElements); +} + + +int main() +{ + int major; + int minor; + int micro; + char version_string[64]; + + bool is_poslog_init_ok = false; + bool is_sns_init_ok = false; + bool is_sns_gyro_init_ok = false; + bool is_sns_accel_init_ok = false; + bool is_gnss_init_ok = false; + int gnss_init_tries = 0; + + registerSigHandlers(); + +#if (DLT_ENABLED) + DLT_REGISTER_APP("GLT","GNSS/SNS Logger"); +#endif + poslogSetFD(STDOUT_FILENO); + is_poslog_init_ok = poslogInit(); + if (is_poslog_init_ok) + { + poslogSetActiveSinks(POSLOG_SINK_DLT|POSLOG_SINK_FD|POSLOG_SINK_CB); + + gnssGetVersion(&major, &minor, µ); + sprintf(version_string, "0,0$GVGNSVER,%d,%d,%d", major, minor, micro); + poslogAddString(version_string); + snsGetVersion(&major, &minor, µ); + sprintf(version_string, "0,0$GVSNSVER,%d,%d,%d", major, minor, micro); + poslogAddString(version_string); + + is_sns_init_ok = snsInit(); + if (is_sns_init_ok) + { + is_sns_gyro_init_ok = snsGyroscopeInit(); + if(is_sns_gyro_init_ok) + { + poslogAddString("#INF snsGyroscopeInit() success"); + snsGyroscopeRegisterCallback(&cbGyro); + } + is_sns_accel_init_ok = snsAccelerationInit(); + if (is_sns_accel_init_ok) + { + poslogAddString("#INF snsAccelerationInit() success"); + snsAccelerationRegisterCallback(&cbAccel); + } + if (!is_sns_gyro_init_ok && !is_sns_accel_init_ok) + { + is_sns_init_ok = false; + snsDestroy(); + } + else + { + poslogAddString("#INF snsInit() success"); + } + } + + //GNSS device may be available a bit late after startup + is_gnss_init_ok = gnssInit(); + while (!is_gnss_init_ok && (gnss_init_tries < GNSS_INIT_MAX_RETRIES) && !sigint && !sigterm) + { + sleep(1); + is_gnss_init_ok = gnssInit(); + gnss_init_tries += 1; + } + if (is_gnss_init_ok) + { + poslogAddString("#INF gnssInit() success"); + gnssRegisterTimeCallback(&cbTime); + gnssRegisterPositionCallback(&cbPosition); + } + + if (is_sns_init_ok || is_gnss_init_ok) + { + while(!sigint && !sigterm) + { + sleep(1); + } + } + else + { + poslogAddString("#ERR snsInit() or gnssInit() failure - terminating"); + } + + //if not interrupted by SIGTERM then we have time to cleanup + if (!sigterm) + { + if (sigint) + { + poslogAddString("#SIGINT"); + } + if (is_sns_init_ok) + { + if (is_sns_accel_init_ok) + { + snsAccelerationDeregisterCallback(&cbAccel); + snsAccelerationDestroy(); + } + if (is_sns_gyro_init_ok) + { + snsGyroscopeRegisterCallback(&cbGyro); + snsGyroscopeDestroy(); + } + snsDestroy(); + } + if (is_gnss_init_ok) + { + gnssDeregisterPositionCallback(&cbPosition); + gnssDeregisterTimeCallback(&cbTime); + gnssDestroy(); + } + } + poslogDestroy(); + } +#if (DLT_ENABLED) + DLT_UNREGISTER_APP(); +#endif +} diff --git a/logger/test/snslog.cpp b/logger/test/snslog.cpp new file mode 100644 index 0000000..57c6943 --- /dev/null +++ b/logger/test/snslog.cpp @@ -0,0 +1,117 @@ +/************************************************************************** +* @licence app begin@ +* +* SPDX-License-Identifier: MPL-2.0 +* +* \brief Utility functions to create and log GNSS specific log strings +* +* +* \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@ +**************************************************************************/ + +#define __STDC_FORMAT_MACROS +#include <inttypes.h> + +#include "snslog.h" +#include "poslog.h" + +#include "gnss.h" + +#include <string.h> +#include <stdio.h> +#include <unistd.h> +#include <time.h> + + + +#define LOG_STRING_SIZE 256 + +uint64_t snslogGetTimestamp() +{ + 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; + } +} + +void accelerationDataToString(uint64_t timestamp, uint16_t countdown, const TAccelerationData* accelerationData, char *str, size_t size) +{ + if ((str) && (size > 0)) + { + snprintf( + str, + size-1, //ensure that there is space for null-terminator + "%"PRIu64",%"PRIu16",$GVSNSACC,%"PRIu64",%7.4f,%7.4f,%7.4f,%5.1f,0X%08X", + timestamp, + countdown, + accelerationData->timestamp, + accelerationData->x, + accelerationData->y, + accelerationData->z, + accelerationData->temperature, + accelerationData->validityBits + ); + str[size-1] = 0; //ensure that string is null-terminated + } +} + +void accelerationDataLog(uint64_t timestamp, const TAccelerationData accelerationData[], uint16_t numElements) +{ + char logstring[LOG_STRING_SIZE] ; + for (int i=0; i<numElements; i++) + { + TPoslogSeq seq = POSLOG_SEQ_CONT; + if (i==0) seq|=POSLOG_SEQ_START; + if (i==(numElements-1)) seq|=POSLOG_SEQ_STOP; + accelerationDataToString(timestamp, numElements-i-1, &accelerationData[i], logstring, LOG_STRING_SIZE); + poslogAddString(logstring, seq); + } +} + +void gyroscopeDataToString(uint64_t timestamp, uint16_t countdown, const TGyroscopeData* gyroData, char *str, size_t size) +{ + if ((str) && (size > 0)) + { + snprintf( + str, + size-1, //ensure that there is space for null-terminator + "%"PRIu64",%"PRIu16",$GVSNSGYRO,%"PRIu64",%6.2f,%6.2f,%6.2f,%5.1f,0X%08X", + timestamp, + countdown, + gyroData->timestamp, + gyroData->yawRate, + gyroData->pitchRate, + gyroData->rollRate, + gyroData->temperature, + gyroData->validityBits + ); + str[size-1] = 0; //ensure that string is null-terminated + } +} + +void gyroscopeDataLog(uint64_t timestamp, const TGyroscopeData gyroData[], uint16_t numElements) +{ + char logstring[LOG_STRING_SIZE] ; + for (int i=0; i<numElements; i++) + { + TPoslogSeq seq = POSLOG_SEQ_CONT; + if (i==0) seq|=POSLOG_SEQ_START; + if (i==(numElements-1)) seq|=POSLOG_SEQ_STOP; + gyroscopeDataToString(timestamp, numElements-i-1, &gyroData[i], logstring, LOG_STRING_SIZE); + poslogAddString(logstring, seq); + } +} diff --git a/logger/test/snslog.h b/logger/test/snslog.h new file mode 100644 index 0000000..b8372d6 --- /dev/null +++ b/logger/test/snslog.h @@ -0,0 +1,88 @@ +/************************************************************************** +* @licence app begin@ +* +* SPDX-License-Identifier: MPL-2.0 +* +* \brief Utility functions to create and log SNS specific log strings +* +* +* \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@ +**************************************************************************/ + +#ifndef INCLUDE_GENIVI_SNS_LOG +#define INCLUDE_GENIVI_SNS_LOG + +#include "poslog.h" +#include "acceleration.h" +#include "gyroscope.h" +#include <string.h> + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Provide a system timestamp in milliseconds. + * @return system timestamp in milliseconds + */ +uint64_t snslogGetTimestamp(); + +/** + * Convert a TAccelerationData structure to a log string. + * @note: The log string will *not* contain a line break (\n) at the end. + * @param timestamp Timestamp for the current time in ms to be added to the header of the log string. + * This timestamp shall be based on the same time source as the timestamp within the accelerationData parameter. + * @param countdown Countdown value to be added to the header of the log string. + * This is the number of following acceleration data in same sequence + * @param accelerationData TAccelerationData structure to be converted to the log string + * @param str Pointer to a string variable where the log string will be written to. + * @param size Size of the string variable where the log string will be written to. + */ +void accelerationDataToString(uint64_t timestamp, uint16_t countdown, const TAccelerationData* accelerationData, char *str, size_t size); + +/** + * Write acceleration data to the position log. + * + * @param timestamp Timestamp when the acceleration data have been received [ms] + * @param accelerationData Pointer to an array of TAccelerationData with size numElements + * @param numElements Number of TAccelerationData elements in array accelerationData + */ +void accelerationDataLog(uint64_t timestamp, const TAccelerationData accelerationData[], uint16_t numElements); + +/** + * Convert a TGyroscopeData structure to a log string. + * @note: The log string will *not* contain a line break (\n) at the end. + * @param timestamp Timestamp for the current time in ms to be added to the header of the log string. + * This timestamp shall be based on the same time source as the timestamp within the gyroData parameter. + * @param countdown Countdown value to be added to the header of the log string. + * This is the number of following acceleration data in same sequence + * @param gyroData TGyroscopeData structure to be converted to the log string + * @param str Pointer to a string variable where the log string will be written to. + * @param size Size of the string variable where the log string will be written to. + */ +void gyroscopeDataToString(uint64_t timestamp, uint16_t countdown, const TGyroscopeData* gyroData, char *str, size_t size); + +/** + * Write acceleration data to the position log. + * + * @param timestamp Timestamp when the acceleration data have been received [ms] + * @param gyroData Pointer to an array of TGyroscopeData with size numElements + * @param numElements Number of TGyroscopeData elements in array gyroData + */ +void gyroscopeDataLog(uint64_t timestamp, const TGyroscopeData gyroData[], uint16_t numElements); + + +#ifdef __cplusplus +} +#endif + +#endif
\ No newline at end of file diff --git a/logger/test/test-poslog.cpp b/logger/test/test-poslog.cpp new file mode 100644 index 0000000..a2fe572 --- /dev/null +++ b/logger/test/test-poslog.cpp @@ -0,0 +1,75 @@ +/************************************************************************** +* @licence app begin@ +* +* SPDX-License-Identifier: MPL-2.0 +* +* \brief Test program for the positioning logger +* +* +* \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@ +**************************************************************************/ + +#include "poslog.h" +#if (DLT_ENABLED) +#include "dlt.h" +#endif +#include <syslog.h> +#include <unistd.h> +#include <string.h> +#include <stdio.h> +#include <pthread.h> + +void testCb(const char* string) +{ + printf("CB %s\n", string); +} + +int main() +{ + char log1[] = "LOG 1"; + char log2[] = "LOG 2"; + const char* logstrings[] = {log1, log2}; + char logger_version_string[64]; + int major; + int minor; + int micro; + EPoslogReleaseLevel level; + + //prepare the sinks: DLT, syslog, file descriptor, callback +#if (DLT_ENABLED) + DLT_REGISTER_APP("PLT","Test Application for Positioning Logging"); +#endif + openlog("POSLOGTEST", LOG_PID, LOG_USER);//syslog + poslogSetFD(STDOUT_FILENO); + poslogSetCB(testCb); + + //Init + poslogInit(); + + poslogGetVersion(&major, &minor, µ, &level); + sprintf(logger_version_string, "0,0$GVLOGVER,%d,%d,%d,%X", major, minor, micro, level); + //activate all possible sinks + poslogSetActiveSinks(POSLOG_SINK_DLT|POSLOG_SINK_SYSLOG|POSLOG_SINK_FD|POSLOG_SINK_CB); + + poslogAddString(logger_version_string); + poslogAddString(logstrings[0],POSLOG_SEQ_START); + poslogAddString(logstrings[1],POSLOG_SEQ_STOP); + + //cleanup + poslogSetActiveSinks(0); + poslogAddString("This log should not appear"); + closelog();//syslog + poslogDestroy(); +#if (DLT_ENABLED) + DLT_UNREGISTER_APP(); +#endif +}
\ No newline at end of file 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, ®, 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<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], ×tamp[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() |