diff --git a/src/cpm.c b/src/cpm.c index 0656028..2cfc5bf 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -48,11 +48,32 @@ 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 + // Create TCP socket s_socket.i32_socket = socket(AF_INET, SOCK_STREAM, 0); if(s_socket.i32_socket < 0){ @@ -66,25 +87,15 @@ 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 ..."); - } - - // 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"); + + if(!waitingIncomingConnection()){ + return false; + } } // Create RAW socket @@ -570,15 +581,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 +597,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 +632,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,9 +679,8 @@ void *cp_service(void *fc){ fmi->itsMessageType = ItsMessageType_cpm; fmi->data.buf = malloc(1500); - /* Creating sockets and waiting for radar to connect*/ - radar_connection(RADAR_PORT,facilities); - + /* Creating sockets and waiting for radar to connect*/ + is_radar_connected = radar_connection(RADAR_PORT,facilities); while(!facilities->exit){ usleep(1000*50); @@ -693,39 +694,43 @@ 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){ - - /* Information parsing from radar */ - parse_input(au8_readBuffer,i32_recv_bytes); + if(is_radar_connected){ + /* 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(); + } + } } ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);