From 4e072faf6788026f12dc40c85b21a287705586a1 Mon Sep 17 00:00:00 2001 From: Marco Correia Date: Mon, 5 Jul 2021 15:56:49 +0100 Subject: [PATCH] Assure connection to the radar when RSU boots up --- src/cpm.c | 81 +++++++++++++++++++++---------------------------------- 1 file changed, 30 insertions(+), 51 deletions(-) diff --git a/src/cpm.c b/src/cpm.c index bdf10fd..5d1efd5 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -49,27 +49,6 @@ S_OBJECT_CONTROL_T s_objectControl; S_OBJECTS_T as_objects[NOF_OBJECTS]; -bool waitingIncomingConnection(void){ - // Listening to the socket (Waiting for incoming connection) - unsigned int len = sizeof(s_socket.s_client); - - if(listen(s_socket.i32_socket,1)<0){ - syslog_err("Waiting for incoming requests failed..."); - return false; - } - - if((s_socket.i32_client = accept(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server, &len)) < 0){ - syslog_err("Client disconnected..."); - return false; - } - - syslog_debug("Radar connected"); - - return true; -} - - - bool radar_connection(char* radar_port, facilities_t* facilities){ if(facilities->dissemination->tmc_connect == false){ @@ -88,14 +67,26 @@ bool radar_connection(char* radar_port, facilities_t* facilities){ s_socket.s_server.sin_addr.s_addr = inet_addr(facilities->dissemination->ip_radar); 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){ + while(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()) + // Listening to the socket (Waiting for incoming connection) + unsigned int len = sizeof(s_socket.s_client); + + if(listen(s_socket.i32_socket,1)<0){ + syslog_err("Waiting for incoming requests failed..."); return false; } + + if((s_socket.i32_client = accept(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server, &len)) < 0){ + syslog_err("Client disconnected..."); + return false; + } + + syslog_debug("Radar connected"); + + } // Create RAW socket @@ -580,12 +571,12 @@ static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len, if(abs(euclidian_dist) > 400 || abs(abs_speed - abs_speed_hist) > 50 || abs(angle_diff) > 4 || abs(generationDeltaTime - history_list[i][4]) >= facilities->dissemination->T_GenCpmMax){ // DEBUG messages - syslog_debug(" DEBUG : [Object ID : %d]",(long)as_objects[i].u8_objectID); + /*syslog_debug(" DEBUG : [Object ID : %d]",(long)as_objects[i].u8_objectID); syslog_debug(" DEBUG : Distance traveled %ld", euclidian_dist ); syslog_debug(" DEBUG : Speed Diff %ld", abs(abs_speed - abs_speed_hist)); syslog_debug(" DEBUG : Angulo: %ld Angulo_hist: %ld | Angle Diff %ld", angle, angle_hist,abs(angle - angle_hist)); syslog_debug(" DEBUG : Time Diff %ld", abs(generationDeltaTime - history_list[i][4])); - syslog_debug(" DEBUG : \n\n\n"); + syslog_debug(" DEBUG : \n\n\n");*/ set_values(i,j,generationDeltaTime,cpm_tx); j++; @@ -602,6 +593,8 @@ static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len, cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.size = j; cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = j; + + //syslog_debug( " DEBUG -------------------------------------------------"); /******* Encode CPMs to FDI and BDR ********/ //BDR @@ -636,7 +629,6 @@ 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; memset(valid_array, 0, sizeof(valid_array)); @@ -678,29 +670,22 @@ void *cp_service(void *fc){ fdi->data.buf = malloc(1500); /* Creating sockets and waiting for radar to connect*/ - is_radar_connected = radar_connection(RADAR_PORT,facilities); + radar_connection(RADAR_PORT,facilities); while(!facilities->exit){ usleep(1000*50); + /* If the Radar is not connected to TMC, a TCP socket is needed to fool the Radar */ + /* To maintain the connection the content must be read */ + if(facilities->dissemination->tmc_connect == false) + i32_recv_bytes = recv(s_socket.i32_client, &au8_readTcp, READ_BUFFER_SIZE, 0); - /* If the Radar is not connected to TMC, a TCP socket is needed to fool the Radar */ - /* To maintain the connection the content must be read */ - if(facilities->dissemination->tmc_connect == false) - i32_recv_bytes = recv(s_socket.i32_client, &au8_readTcp, READ_BUFFER_SIZE, 0); + /* Reads from the radar */ + i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0); - /* Reads from the radar */ - i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0); - - if (dissemination_check(facilities->dissemination, &facilities->epv,1) && facilities->dissemination->active){ - if(is_radar_connected){ - if(i32_recv_bytes <= 0){ - syslog_debug("No data received from radar ..."); - is_radar_connected = false; - continue; - } - + if (dissemination_check(facilities->dissemination, &facilities->epv,1) && facilities->dissemination->active){ + /* Information parsing from radar */ parse_input(au8_readBuffer,i32_recv_bytes); @@ -730,15 +715,9 @@ void *cp_service(void *fc){ queue_add(facilities->tx_queue, fdi_oer, enc_fdi.encoded+1, 5); pthread_cond_signal(&facilities->tx_queue->trigger); - /*Reset Timer for dissemination control */ - + /*Reset Timer for dissemination control */ dissemination_reset_timer(facilities->dissemination, &facilities->epv,1); - - - } else { /* Waiting for Radar to reconnect */ - is_radar_connected = waitingIncomingConnection(); - } - } + } } ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr);