From 11f519fe671c43f74dbdf1958014555f57f52f97 Mon Sep 17 00:00:00 2001 From: Marco Date: Wed, 26 May 2021 10:59:42 +0100 Subject: [PATCH] CPM generation can work without TMC software --- src/config.c | 5 +++ src/cpm.c | 109 +++++++++++++++++++++++++++++++++++++-------------- src/cpm.h | 14 +++++-- 3 files changed, 95 insertions(+), 33 deletions(-) diff --git a/src/config.c b/src/config.c index 90138e2..7272188 100644 --- a/src/config.c +++ b/src/config.c @@ -249,6 +249,11 @@ int facilities_config(void* facilities_s) { facilities->dissemination->T_GenCpmMin = config->facilities.cpm.rsu_obu_period_min; facilities->dissemination->T_GenCpmMax = config->facilities.cpm.rsu_obu_period_max; facilities->dissemination->radar_rotation = config->applications.its_center.rotation; + facilities->dissemination->tmc_connect = config->facilities.cpm.tmc_connected; + facilities->dissemination->latitude = config->management.gps.latitude; + facilities->dissemination->longitude = config->management.gps.longitude; + + // Replay facilities->replay = config->networking.replay.activate; diff --git a/src/cpm.c b/src/cpm.c index 12f09f1..dda2afa 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -17,6 +17,8 @@ #include #include #include +#include +#include #include @@ -38,6 +40,7 @@ float roadRotationSin; float roadRotationCos; S_ETHERNET_CONNECTION_T s_socket; +S_INTERFACE_CONNECTION_T raw_socket; S_OBJECT_CONTROL_T s_objectControl; S_OBJECTS_T as_objects[NOF_OBJECTS]; @@ -63,29 +66,64 @@ bool waitingIncomingConnection(void){ -bool interface_connection(char* radar_ip,char* radar_port){ +bool interface_connection(char* radar_ip,char* radar_port, facilities_t* facilities){ - // 1 - Create socket - s_socket.i32_socket = socket(AF_INET, SOCK_STREAM, 0); + if(facilities->dissemination->tmc_connect == false){ + + // 1 - Create socket + s_socket.i32_socket = socket(AF_INET, SOCK_STREAM, 0); - if(s_socket.i32_socket < 0){ - syslog_err("Initializing socket failed ..."); - return false; - } + if(s_socket.i32_socket < 0){ + syslog_err("Initializing socket failed ..."); + return false; + } - // 2 - Bind it to server address and port - bzero(&s_socket.s_server, sizeof(s_socket.s_server)); - s_socket.s_server.sin_family = AF_INET; - s_socket.s_server.sin_addr.s_addr = inet_addr(radar_ip); - s_socket.s_server.sin_port = htons(atoi(radar_port)); + // 2 - Bind it to server address and port + bzero(&s_socket.s_server, sizeof(s_socket.s_server)); + s_socket.s_server.sin_family = AF_INET; + s_socket.s_server.sin_addr.s_addr = inet_addr(radar_ip); + s_socket.s_server.sin_port = htons(atoi(radar_port)); - if(bind(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server,sizeof(s_socket.s_server)) < 0){ - syslog_err("Binding socket to address error ..."); - return false; + if(bind(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server,sizeof(s_socket.s_server)) < 0){ + syslog_err("Binding socket to address error ..."); + return false; + } + + if(!waitingIncomingConnection()) + return false; + } + + // Create RAW socket + + raw_socket.raw_fd = socket(AF_PACKET, SOCK_RAW, htons(0x0800)); + + if(raw_socket.raw_fd < 0){ + syslog_err("Failed to initializing RAW socket ..."); + return false; + } + + // Get interface index + + bzero(&raw_socket.sll, sizeof(raw_socket.sll)); + bzero(&raw_socket.ifr, sizeof(raw_socket.ifr)); + + strncpy((char *)raw_socket.ifr.ifr_name, INTERFACE_RADAR, IFNAMSIZ); + if((ioctl(raw_socket.raw_fd, SIOCGIFINDEX, &raw_socket.ifr)) == -1){ + syslog_err("Error getting interface index"); + return false; } - if(!waitingIncomingConnection()) + raw_socket.sll.sll_family = AF_PACKET; + raw_socket.sll.sll_ifindex = raw_socket.ifr.ifr_ifindex; + raw_socket.sll.sll_protocol = htons(0x0800); + + // Bind it to the interface + + if(bind(raw_socket.raw_fd, (struct sockaddr *)&raw_socket.sll, sizeof(raw_socket.sll))<0){ + syslog_err("Error binding RAW socket ..."); return false; + } + return true; @@ -377,8 +415,8 @@ void mk_cpm(facilities_t* facilities,CPM_t* cpm_tx, struct timespec* systemtime) cpm_tx->cpm.generationDeltaTime = generationDeltaTime; cpm_tx->cpm.cpmParameters.managementContainer.stationType = StationType_roadSideUnit; - cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.latitude = 420309860; //A3 // 412400078; (A24) - cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = -86515778; //A3 // -86950224; (A24) + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.latitude = facilities->dissemination->latitude; + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = facilities->dissemination->longitude; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 100; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 100; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_wgs84North; @@ -498,20 +536,23 @@ void dissemination_reset_timer(dissemination_t* dissemination, it2s_tender_epv_t } -void *cp_service(void *fc){ - +void *cp_service(void *fc){ + /* Variables */ int i32_recv_bytes; u_int8_t au8_readBuffer[READ_BUFFER_SIZE]; bool is_radar_connected; - + u_int8_t au8_readTcp[READ_BUFFER_SIZE]; + facilities_t *facilities = (facilities_t *) fc; + + + uint8_t bdr_oer[2048]; uint8_t fdi_oer[2048]; bdr_oer[0] = 4; //Facilities fdi_oer[0] = 4; - facilities_t *facilities = (facilities_t *) fc; BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); FacilitiesDataIndication_t *fdi = calloc(1, sizeof(FacilitiesDataIndication_t)); @@ -540,7 +581,7 @@ void *cp_service(void *fc){ *bdr->gnSecurityProfile = 1; } - is_radar_connected = interface_connection(RADAR_IP,RADAR_PORT); // Create Radar listening socket + is_radar_connected = interface_connection(RADAR_IP,RADAR_PORT,facilities); // Create Radar listening socket /*--- Fill mandatory Facilities Data Indication parameters ---*/ @@ -559,11 +600,17 @@ void *cp_service(void *fc){ while(!facilities->exit){ usleep(1000*50); - - /* Receive Data from radar interface */ - CPM_t* cpm_tx = calloc(1, sizeof(CPM_t)); - i32_recv_bytes = recv(s_socket.i32_client, &au8_readBuffer, READ_BUFFER_SIZE, 0); //recv(socket,buffer,size,flags) + + /* If the Radar is not connected to TMC, a TCP socket is needed to fool the Radar */ + if(facilities->dissemination->tmc_connect == false) + i32_recv_bytes = recv(s_socket.i32_client, &au8_readTcp, READ_BUFFER_SIZE, 0); + + /* Reads from the radar */ + CPM_t* cpm_tx = calloc(1, sizeof(CPM_t)); + i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0); //recv(socket,buffer,size,flags) + + if (dissemination_check(facilities->dissemination, &facilities->epv) && facilities->dissemination->active){ if(is_radar_connected){ @@ -572,12 +619,14 @@ void *cp_service(void *fc){ is_radar_connected = false; continue; } - + + /* Information parsing */ parse_input(au8_readBuffer,i32_recv_bytes); /* CPM build and encoding to bdr and fdi (Could be merged if needed) */ - mk_cpm(facilities,cpm_tx,systemtime); + mk_cpm(facilities,cpm_tx,systemtime); + if(encode_cpm(cpm_tx, bdr->data.buf, (uint32_t *) &bdr->data.size) == 1){ ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); @@ -621,7 +670,7 @@ void *cp_service(void *fc){ } else { /* Waiting for Radar to reconnect */ - is_radar_connected = waitingIncomingConnection(); + is_radar_connected = waitingIncomingConnection(); } } } diff --git a/src/cpm.h b/src/cpm.h index 6fc5f0d..77481d2 100644 --- a/src/cpm.h +++ b/src/cpm.h @@ -17,8 +17,9 @@ #define ROAD_ANGLE 120 #define MESSAGE_ID 14 #define PROTOCOL_VERSION 1 +#define INTERFACE_RADAR "enp3s0" -#define RADAR_IP "11.11.11.13" // ITS-S interface connected to the radar +#define RADAR_IP "192.168.11.1" // ITS-S interface connected to the radar #define RADAR_PORT "55555" // Destination port from the radar @@ -118,10 +119,11 @@ typedef struct { bool active; + bool tmc_connect; pthread_mutex_t lock; - uint8_t type; + uint32_t type; uint64_t next_cpm_max; uint64_t next_cpm_min; @@ -130,9 +132,15 @@ typedef struct uint64_t T_GenCpmMin; uint64_t T_GenCpmMax; + /* Position of the radar (Value from toml) */ - int8_t radar_rotation; + int64_t radar_rotation; + +/* Fixed GPS coordenates */ + + double latitude; + double longitude; } dissemination_t;