Merge branch 'master' of https://gitlab.es.av.it.pt/its/it2s-itss-facilities

src/cpm.c

@@ -48,11 +48,32 @@ 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){
 
-		// 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...");
+		if(!waitingIncomingConnection()){
 			return false;
 		}
-	
-		syslog_debug("Radar connected");
-
 	}
 
 	// 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));
 
@@ -678,8 +680,7 @@ void *cp_service(void *fc){
 	fmi->data.buf = malloc(1500);
 	
 	/* Creating sockets and waiting for radar to connect*/	
-	radar_connection(RADAR_PORT,facilities);
+	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){
+			if(is_radar_connected){
+				/* Information parsing from radar */
+				parse_input(au8_readBuffer,i32_recv_bytes);																				
     	
-			/* Information parsing from radar */
+    				/* CPM build and encoding to BDR and FDI */ 						
-			parse_input(au8_readBuffer,i32_recv_bytes);																				
+		    		if(mk_cpm(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size) == 1)
+					continue;				
 							
-    			/* CPM build and encoding to BDR and FDI */ 						
+				/* Encode TransportRequest */
-	    		if(mk_cpm(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size) == 1)
+				asn_enc_rval_t enc_tdr = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 2047);
-				continue;				
+				if(enc_tdr.encoded == -1){
+					syslog_err("[facilities] encoding TR for cpm failed");
+					continue;
+				}
 
-			/* Encode TransportRequest */
+				/* Encode FacilitiesIndication */ 
-			asn_enc_rval_t enc_tdr = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 2047);
+				asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, fi_oer+1, 2047);
-			if(enc_tdr.encoded == -1){
+				if(enc_fdi.encoded == -1){
-				syslog_err("[facilities] encoding TR for cpm failed");
+					syslog_err("[facilities] encoding FI for cpm failed");
-				continue;
+					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 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();
 			}	
-
+		}
-			/* 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 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);
-    		}    
 	}
 
     ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);