diff options
Diffstat (limited to 'src/sensors-service/sns-use-replayer.c')
| -rw-r--r-- | src/sensors-service/sns-use-replayer.c | 517 |
1 files changed, 517 insertions, 0 deletions
diff --git a/src/sensors-service/sns-use-replayer.c b/src/sensors-service/sns-use-replayer.c new file mode 100644 index 0000000..a628453 --- /dev/null +++ b/src/sensors-service/sns-use-replayer.c @@ -0,0 +1,517 @@ +/************************************************************************** +* @licence app begin@ +* +* SPDX-License-Identifier: MPL-2.0 +* +* \ingroup SensorsService +* \author Marco Residori <marco.residori@xse.de> +* +* \copyright Copyright (C) 2013, XS Embedded GmbH +* +* \license +* This Source Code Form is subject to the terms of the +* Mozilla Public License, v. 2.0. If a copy of the MPL was not distributed with +* this file, You can obtain one at http://mozilla.org/MPL/2.0/. +* +* @licence end@ +**************************************************************************/ + +#include "sns-init.h" +#include "acceleration.h" +#include "gyroscope.h" +#include "inclination.h" +#include "odometer.h" +#include "reverse-gear.h" +#include "slip-angle.h" +#include "steering-angle.h" +#include "vehicle-data.h" +#include "vehicle-speed.h" +#include "vehicle-state.h" +#include "wheel.h" + +#include <stdio.h> +#include <stdlib.h> +#include <stdint.h> +#include <stdbool.h> +#include <time.h> +#include <errno.h> +#include <pthread.h> +#include <assert.h> + +#include <arpa/inet.h> +#include <netinet/in.h> +#include <sys/types.h> +#include <sys/socket.h> +#include <unistd.h> +#include <memory.h> + +#include "globals.h" +#include "log.h" + +#define STRINGIFY2( x) #x +#define STRINGIFY(x) STRINGIFY2(x) + +#define BUFLEN 256 +#define MSGIDLEN 20 +#define PORT 9931 + +#define MAX_BUF_MSG 16 + +DLT_DECLARE_CONTEXT(gContext); + +//Listener thread +static pthread_t listenerThread; +//Listener thread loop control variale +static volatile bool isRunning = false; +//Socket file descriptor used by listener thread. +//Global so we can shutdown() it to release listener thread immediately from waiting +static int s = 0; +//Note: we do not mutex-protect the above globals because for this proof-of-concept +//implementation we expect that the client does not ake overlapping calls. +//For a real-world fool-proof implementation you would have to add more checks. + + +static void *listenForMessages( void *ptr ); + +bool snsInit() +{ + isRunning = true; + + if(pthread_create( &listenerThread, NULL, listenForMessages, NULL) != 0) + { + isRunning = false; + return false; + } + + return true; +} + +bool snsDestroy() +{ + isRunning = false; + + //shut down the socket + shutdown(s,2); + + if(listenerThread) + { + pthread_join( listenerThread, NULL); + } + + return true; +} + +void snsGetVersion(int *major, int *minor, int *micro) +{ + if(major) + { + *major = GENIVI_SNS_API_MAJOR; + } + + if(minor) + { + *minor = GENIVI_SNS_API_MINOR; + } + + if(micro) + { + *micro = GENIVI_SNS_API_MICRO; + } +} + +bool snsAccelerationInit() +{ + return iAccelerationInit(); +} + +bool snsAccelerationDestroy() +{ + return iAccelerationDestroy(); +} + +bool snsGyroscopeInit() +{ + return iGyroscopeInit(); +} + +bool snsGyroscopeDestroy() +{ + return iGyroscopeDestroy(); +} + +bool snsVehicleSpeedInit() +{ + return iVehicleSpeedInit(); +} + +bool snsVehicleSpeedDestroy() +{ + return iVehicleSpeedDestroy(); +} + +bool snsWheelInit() +{ + return iWheelInit(); +} + +bool snsWheelDestroy() +{ + return iWheelDestroy(); +} + +bool snsVehicleDataInit() +{ + return iVehicleDataInit(); +} + +bool snsVehicleDataDestroy() +{ + return iVehicleDataDestroy(); +} + +bool processGVSNSWHE(const char* data) +{ + //parse data like: 15259,0,$GVSNSWHE,15200,35,45,0,0,0,0,0,0,0X0001,100,0X03 + + //storage for buffered data + static TWheelData buf_whtk[MAX_BUF_MSG]; + static uint16_t buf_size = 0; + static uint16_t last_countdown = 0; + + uint64_t timestamp; + uint16_t countdown; + TWheelData whtk = { 0 }; + int n = 0; + + if(!data) + { + LOG_ERROR_MSG(gContext,"wrong parameter!"); + return false; + } + + //First try to read in new format with measurementInterval + n = sscanf(data, "%llu,%hu,$GVSNSWHE,%llu,%f,%f,%f,%f,%f,%f,%f,%f,%x,%u,%x", + ×tamp, &countdown, &whtk.timestamp + ,&whtk.data[0], &whtk.data[1] + ,&whtk.data[2], &whtk.data[3] + ,&whtk.data[4], &whtk.data[5] + ,&whtk.data[6], &whtk.data[7] + ,&whtk.statusBits + ,&whtk.measurementInterval + ,&whtk.validityBits + ); + + if (n != 14) //14 fields to parse + { + //Else try to read in old format without measurementInterval + n = sscanf(data, "%llu,%hu,$GVSNSWHE,%llu,%f,%f,%f,%f,%f,%f,%f,%f,%x,%x", + ×tamp, &countdown, &whtk.timestamp + ,&whtk.data[0], &whtk.data[1] + ,&whtk.data[2], &whtk.data[3] + ,&whtk.data[4], &whtk.data[5] + ,&whtk.data[6], &whtk.data[7] + ,&whtk.statusBits + ,&whtk.validityBits + ); + + if (n != 13) //13 fields to parse + { + LOG_ERROR_MSG(gContext,"replayer: processGVSNSWHE failed!"); + return false; + } + } + + //buffered data handling + if (countdown < MAX_BUF_MSG) //enough space in buffer? + { + if (buf_size == 0) //a new sequence starts + { + buf_size = countdown+1; + last_countdown = countdown; + buf_whtk[buf_size-countdown-1] = whtk; + } + else if ((last_countdown-countdown) == 1) //sequence continued + { + last_countdown = countdown; + buf_whtk[buf_size-countdown-1] = whtk; + } + else //sequence interrupted: clear buffer + { + buf_size = 0; + last_countdown = 0; + } + } + else //clear buffer + { + buf_size = 0; + last_countdown = 0; + } + + if((countdown == 0) && (buf_size >0) ) + { + updateWheelData(buf_whtk, buf_size); + buf_size = 0; + last_countdown = 0; + } + + return true; +} + +static bool processGVSNSGYR(const char* data) +{ + //parse data like: 061074000,0$GVSNSGYR,061074000,-38.75,0,0,0,0X01 + + //storage for buffered data + static TGyroscopeData buf_gyro[MAX_BUF_MSG]; + static uint16_t buf_size = 0; + static uint16_t last_countdown = 0; + + uint64_t timestamp; + uint16_t countdown; + TGyroscopeData gyro = { 0 }; + int n = 0; + + if(!data ) + { + LOG_ERROR_MSG(gContext,"wrong parameter!"); + return false; + } + + //First try to read in new format with measurementInterval + n = sscanf(data, "%llu,%hu,$GVSNSGYR,%llu,%f,%f,%f,%f,%u,%x" + ,×tamp, &countdown, &gyro.timestamp + ,&gyro.yawRate + ,&gyro.pitchRate + ,&gyro.rollRate + ,&gyro.temperature + ,&gyro.measurementInterval + ,&gyro.validityBits + ); + if (n != 9) //9 fields to parse + { + //Else try to read in old format without measurementInterval + n = sscanf(data, "%llu,%hu,$GVSNSGYR,%llu,%f,%f,%f,%f,%x" + ,×tamp, &countdown, &gyro.timestamp + ,&gyro.yawRate + ,&gyro.pitchRate + ,&gyro.rollRate + ,&gyro.temperature + ,&gyro.validityBits + ); + + if (n != 8) //8 fields to parse + { + LOG_ERROR_MSG(gContext,"replayer: processGVSNSGYR failed!"); + return false; + } + } + + //buffered data handling + if (countdown < MAX_BUF_MSG) //enough space in buffer? + { + if (buf_size == 0) //a new sequence starts + { + buf_size = countdown+1; + last_countdown = countdown; + buf_gyro[buf_size-countdown-1] = gyro; + } + else if ((last_countdown-countdown) == 1) //sequence continued + { + last_countdown = countdown; + buf_gyro[buf_size-countdown-1] = gyro; + } + else //sequence interrupted: clear buffer + { + buf_size = 0; + last_countdown = 0; + } + } + else //clear buffer + { + buf_size = 0; + last_countdown = 0; + } + + if((countdown == 0) && (buf_size >0) ) + { + updateGyroscopeData(buf_gyro,buf_size); + buf_size = 0; + last_countdown = 0; + } + + return true; +} + + + +static bool processGVSNSVSP(const char* data) +{ + //parse data like: 061074000,0$GVSNSVSP,061074000,0.51,0X01 + + //storage for buffered data + static TVehicleSpeedData buf_vehsp[MAX_BUF_MSG]; + static uint16_t buf_size = 0; + static uint16_t last_countdown = 0; + + uint64_t timestamp; + uint16_t countdown; + TVehicleSpeedData vehsp = { 0 }; + int n = 0; + + if(!data) + { + LOG_ERROR_MSG(gContext,"wrong parameter!"); + return false; + } + + //First try to read in new format with measurementInterval + n = sscanf(data, "%llu,%hu,$GVSNSVSP,%llu,%f,%u,%x" + ,×tamp + ,&countdown + ,&vehsp.timestamp + ,&vehsp.vehicleSpeed + ,&vehsp.measurementInterval + ,&vehsp.validityBits + ); + + if (n != 6) //6 fields to parse + { + //Else try to read in old format without measurementInterval + n = sscanf(data, "%llu,%hu,$GVSNSVSP,%llu,%f,%x" + ,×tamp + ,&countdown + ,&vehsp.timestamp + ,&vehsp.vehicleSpeed + ,&vehsp.validityBits + ); + + if (n != 5) //5 fields to parse + { + LOG_ERROR_MSG(gContext,"replayer: processGVSNSVSP failed!"); + return false; + } + } + + //buffered data handling + if (countdown < MAX_BUF_MSG) //enough space in buffer? + { + if (buf_size == 0) //a new sequence starts + { + buf_size = countdown+1; + last_countdown = countdown; + buf_vehsp[buf_size-countdown-1] = vehsp; + } + else if ((last_countdown-countdown) == 1) //sequence continued + { + last_countdown = countdown; + buf_vehsp[buf_size-countdown-1] = vehsp; + } + else //sequence interrupted: clear buffer + { + buf_size = 0; + last_countdown = 0; + } + } + else //clear buffer + { + buf_size = 0; + last_countdown = 0; + } + + if((countdown == 0) && (buf_size >0) ) + { + updateVehicleSpeedData(buf_vehsp,buf_size); + buf_size = 0; + last_countdown = 0; + } + + return true; +} + +static void *listenForMessages( void *ptr ) +{ + struct sockaddr_in si_me; + struct sockaddr_in si_other; + socklen_t slen = sizeof(si_other); + ssize_t readBytes = 0; + char buf[BUFLEN+1]; //add space fer terminating \0 + char msgId[MSGIDLEN+1]; //add space fer terminating \0 + int port = PORT; + + DLT_REGISTER_APP("SNSS", "SENSOSRS-SERVICE"); + DLT_REGISTER_CONTEXT(gContext,"SSRV", "Global Context"); + + LOG_INFO(gContext,"SensorsService listening on port %d...",port); + + if((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) + { + LOG_ERROR_MSG(gContext,"socket() failed!"); + exit(EXIT_FAILURE); + } + + memset((char *) &si_me, 0, sizeof(si_me)); + si_me.sin_family = AF_INET; + + si_me.sin_port = htons(port); + si_me.sin_addr.s_addr = htonl(INADDR_ANY); + if(bind(s, (struct sockaddr *)&si_me, sizeof(si_me)) == -1) + { + LOG_ERROR_MSG(gContext,"socket() failed!"); + exit(EXIT_FAILURE); + } + + while(isRunning == true) + { + //use select to introduce a timeout - alloy shutdown even when no data are received + fd_set readfs; /* file descriptor set */ + int maxfd; /* maximum file desciptor used */ + int res; + struct timeval Timeout; + /* set timeout value within input loop */ + Timeout.tv_usec = 0; /* milliseconds */ + Timeout.tv_sec = 1; /* seconds */ + FD_SET(s, &readfs); + maxfd = s+1; + /* block until input becomes available */ + res = select(maxfd, &readfs, NULL, NULL, &Timeout); + + if (res > 0) + { + + readBytes = recvfrom(s, buf, BUFLEN, 0, (struct sockaddr *)&si_other, &slen); + + if(readBytes < 0) + { + LOG_ERROR_MSG(gContext,"recvfrom() failed!"); + exit(EXIT_FAILURE); + } + + buf[readBytes] = '\0'; + + sscanf(buf, "%*[^'$']$%" STRINGIFY(MSGIDLEN) "[^',']", msgId); + + LOG_DEBUG(gContext,"Data:%s", buf); + + if(strcmp("GVSNSGYR", msgId) == 0) + { + processGVSNSGYR(buf); + } + else if(strcmp("GVSNSWHE", msgId) == 0) + { + processGVSNSWHE(buf); + } + else if(strcmp("GVSNSVSP", msgId) == 0) + { + processGVSNSVSP(buf); + } + } + } + + close(s); + + return EXIT_SUCCESS; +} + + + + |