TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Assure connection to the radar when RSU boots up

This commit is contained in:
Marco Correia 2021-07-05 15:56:49 +01:00
parent 6575744cd3
commit 4e072faf67
1 changed files with 30 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
src/cpm.c
View File

@ -49,27 +49,6 @@ S_OBJECT_CONTROL_T s_objectControl;
S_OBJECTS_T as_objects[NOF_OBJECTS]; 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){ bool radar_connection(char* radar_port, facilities_t* facilities){
if(facilities->dissemination->tmc_connect == false){ if(facilities->dissemination->tmc_connect == false){
@ -88,15 +67,27 @@ 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_addr.s_addr = inet_addr(facilities->dissemination->ip_radar);
s_socket.s_server.sin_port = htons(atoi(radar_port)); 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 ..."); 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; return false;
} }
if(!waitingIncomingConnection()) if((s_socket.i32_client = accept(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server, &len)) < 0){
syslog_err("Client disconnected...");
return false; return false;
} }
syslog_debug("Radar connected");
}
// Create RAW socket // Create RAW socket
raw_socket.raw_fd = socket(AF_PACKET, SOCK_RAW, htons(0x0800)); raw_socket.raw_fd = socket(AF_PACKET, SOCK_RAW, htons(0x0800));
@ -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){ 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 // 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 : Distance traveled %ld", euclidian_dist );
syslog_debug(" DEBUG : Speed Diff %ld", abs(abs_speed - abs_speed_hist)); 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 : 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 : 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); set_values(i,j,generationDeltaTime,cpm_tx);
j++; 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.perceivedObjectContainer->list.size = j;
cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = j; cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = j;
//syslog_debug( " DEBUG -------------------------------------------------");
/******* Encode CPMs to FDI and BDR ********/ /******* Encode CPMs to FDI and BDR ********/
//BDR //BDR
@ -636,7 +629,6 @@ void *cp_service(void *fc){
/* Variables */ /* Variables */
int i32_recv_bytes; int i32_recv_bytes;
u_int8_t au8_readBuffer[READ_BUFFER_SIZE]; u_int8_t au8_readBuffer[READ_BUFFER_SIZE];
bool is_radar_connected;
u_int8_t au8_readTcp[READ_BUFFER_SIZE]; u_int8_t au8_readTcp[READ_BUFFER_SIZE];
facilities_t *facilities = (facilities_t *) fc; facilities_t *facilities = (facilities_t *) fc;
memset(valid_array, 0, sizeof(valid_array)); memset(valid_array, 0, sizeof(valid_array));
@ -678,28 +670,21 @@ void *cp_service(void *fc){
fdi->data.buf = malloc(1500); fdi->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);
while(!facilities->exit){ while(!facilities->exit){
usleep(1000*50); 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 */ /* Reads from the radar */
/* To maintain the connection the content must be read */ i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0);
if(facilities->dissemination->tmc_connect == false)
i32_recv_bytes = recv(s_socket.i32_client, &au8_readTcp, READ_BUFFER_SIZE, 0);
/* Reads from the radar */ if (dissemination_check(facilities->dissemination, &facilities->epv,1) && facilities->dissemination->active){
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;
}
/* Information parsing from radar */ /* Information parsing from radar */
parse_input(au8_readBuffer,i32_recv_bytes); parse_input(au8_readBuffer,i32_recv_bytes);
@ -731,14 +716,8 @@ void *cp_service(void *fc){
pthread_cond_signal(&facilities->tx_queue->trigger); 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); 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); ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr);