From 1e3765e53b9b959a13a17a7489a5995154091814 Mon Sep 17 00:00:00 2001 From: Marco Correia Date: Tue, 24 Aug 2021 17:35:00 +0100 Subject: [PATCH 1/2] Revoked 4e072faf. Assure connection to the Radar --- src/cpm.c | 53 ++++++++++++++++++++++++++++++++++++++--------------- 1 file changed, 38 insertions(+), 15 deletions(-) diff --git a/src/cpm.c b/src/cpm.c index 0656028..cf9ecce 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -48,8 +48,29 @@ S_OBJECT_CONTROL_T s_objectControl; S_OBJECTS_T as_objects[NOF_OBJECTS]; -bool radar_connection(char* radar_port, facilities_t* facilities){ + +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){ // Create socket @@ -66,8 +87,14 @@ 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)); - while(bind(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server,sizeof(s_socket.s_server)) < 0){ + 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; } // Listening to the socket (Waiting for incoming connection) @@ -570,15 +597,6 @@ static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len, // Requirements to include the object in the CPM (> 4 m or > 0.5 m/s or > 4ยบ or > T_GenCpmMax) 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 : 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");*/ - set_values(i,j,generationDeltaTime,cpm_tx); j++; @@ -595,7 +613,6 @@ static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len, cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = j; - //syslog_debug( " DEBUG -------------------------------------------------"); /******* Encode CPMs to FDI and BDR ********/ //BDR @@ -631,6 +648,7 @@ void *cp_service(void *fc){ int i32_recv_bytes; u_int8_t au8_readBuffer[READ_BUFFER_SIZE]; u_int8_t au8_readTcp[READ_BUFFER_SIZE]; + bool is_radar_connected; facilities_t *facilities = (facilities_t *) fc; memset(valid_array, 0, sizeof(valid_array)); @@ -677,7 +695,8 @@ void *cp_service(void *fc){ fmi->itsMessageType = ItsMessageType_cpm; fmi->data.buf = malloc(1500); - /* Creating sockets and waiting for radar to connect*/ + /* Creating sockets and waiting for radar to connect*/ + is_radar_connected = radar_connection(RADAR_PORT,facilities); radar_connection(RADAR_PORT,facilities); @@ -693,7 +712,7 @@ void *cp_service(void *fc){ 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){ /* Information parsing from radar */ parse_input(au8_readBuffer,i32_recv_bytes); @@ -725,7 +744,11 @@ void *cp_service(void *fc){ /*Reset Timer for dissemination control */ dissemination_reset_timer(facilities->dissemination, &facilities->epv,1); - } + + }else{ + is_radar_connected = waitingIncomingConnection(); + } + } } ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); From 44135c488262242a9b8237ec7aeb01b1bc2d38ba Mon Sep 17 00:00:00 2001 From: Marco Correia Date: Fri, 27 Aug 2021 18:14:06 +0100 Subject: [PATCH 2/2] Changes to last commit --- src/cpm.c | 76 +++++++++++++++++++++---------------------------------- 1 file changed, 29 insertions(+), 47 deletions(-) diff --git a/src/cpm.c b/src/cpm.c index cf9ecce..2cfc5bf 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -73,7 +73,7 @@ bool radar_connection(char* radar_port, facilities_t* facilities){ if(facilities->dissemination->tmc_connect == false){ - // Create socket + // Create TCP socket s_socket.i32_socket = socket(AF_INET, SOCK_STREAM, 0); if(s_socket.i32_socket < 0){ @@ -96,22 +96,6 @@ bool radar_connection(char* radar_port, facilities_t* facilities){ if(!waitingIncomingConnection()){ return false; } - - // 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 @@ -697,8 +681,6 @@ void *cp_service(void *fc){ /* 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); @@ -713,37 +695,37 @@ void *cp_service(void *fc){ if (dissemination_check(facilities->dissemination, &facilities->epv,1) && facilities->dissemination->active){ if(is_radar_connected){ - /* Information parsing from radar */ - parse_input(au8_readBuffer,i32_recv_bytes); + /* Information parsing from radar */ + parse_input(au8_readBuffer,i32_recv_bytes); - /* CPM build and encoding to BDR and FDI */ - if(mk_cpm(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size) == 1) - continue; - - /* Encode TransportRequest */ - asn_enc_rval_t enc_tdr = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 2047); - if(enc_tdr.encoded == -1){ - syslog_err("[facilities] encoding TR for cpm failed"); - continue; - } + /* CPM build and encoding to BDR and FDI */ + if(mk_cpm(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size) == 1) + continue; + + /* Encode TransportRequest */ + asn_enc_rval_t enc_tdr = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 2047); + if(enc_tdr.encoded == -1){ + syslog_err("[facilities] encoding TR for cpm failed"); + continue; + } - /* Encode FacilitiesIndication */ - asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, fi_oer+1, 2047); - if(enc_fdi.encoded == -1){ - syslog_err("[facilities] encoding FI for cpm failed"); - continue; - } + /* Encode FacilitiesIndication */ + asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, fi_oer+1, 2047); + if(enc_fdi.encoded == -1){ + syslog_err("[facilities] encoding FI for cpm failed"); + continue; + } + + /* Create thread to send packet to the Transport Layer (=3) */ + queue_add(facilities->tx_queue, tr_oer, enc_tdr.encoded+1, 3); + pthread_cond_signal(&facilities->tx_queue->trigger); - /* Create thread to send packet to the Transport Layer (=3) */ - queue_add(facilities->tx_queue, tr_oer, enc_tdr.encoded+1, 3); - pthread_cond_signal(&facilities->tx_queue->trigger); - - /* Create thread to send packet to the Applications Layer (=5) */ - queue_add(facilities->tx_queue, fi_oer, enc_fdi.encoded+1, 5); - pthread_cond_signal(&facilities->tx_queue->trigger); - - /*Reset Timer for dissemination control */ - dissemination_reset_timer(facilities->dissemination, &facilities->epv,1); + /* Create thread to send packet to the Applications Layer (=5) */ + queue_add(facilities->tx_queue, fi_oer, enc_fdi.encoded+1, 5); + pthread_cond_signal(&facilities->tx_queue->trigger); + + /*Reset Timer for dissemination control */ + dissemination_reset_timer(facilities->dissemination, &facilities->epv,1); }else{ is_radar_connected = waitingIncomingConnection();