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

Added the ability for radar to reconnect (CPM)

This commit is contained in:
Marco 2021-04-23 15:47:39 +01:00
parent 88015dc131
commit ae5701bb87
1 changed files with 93 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

175
src/cpm.c
View File

@ -11,6 +11,7 @@
#include <signal.h> #include <signal.h>
#include <unistd.h> #include <unistd.h>
#include <syslog.h> #include <syslog.h>
#include <arpa/inet.h> #include <arpa/inet.h>
#include <stdbool.h> #include <stdbool.h>
@ -41,6 +42,26 @@ char* station_id;
it2s_config_t* config; it2s_config_t* config;
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 interface_connection(char* radar_ip,char* radar_port){ bool interface_connection(char* radar_ip,char* radar_port){
@ -63,26 +84,14 @@ bool interface_connection(char* radar_ip,char* radar_port){
return false; return false;
} }
if(!waitingIncomingConnection())
// 3 - 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((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; return true;
}
}
void parse_can_data_tm(u_int32_t u32_can_id, int i32_can_len, u_int8_t* au8_can_data) { void parse_can_data_tm(u_int32_t u32_can_id, int i32_can_len, u_int8_t* au8_can_data) {
u_int8_t u8_pvrMessagePart = 0; u_int8_t u8_pvrMessagePart = 0;
u_int8_t tmp = 0; u_int8_t tmp = 0;
@ -511,107 +520,109 @@ void dissemination_reset_timer(dissemination_t* dissemination){
void *cp_service(void *fc){ 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];
//void* message_data = malloc(1500); uint8_t bdr_oer[2048];
uint8_t bdr_oer[2048]; bdr_oer[0] = 4; //Facilities
bdr_oer[0] = 4; //Facilities bool is_radar_connected;
int is_radar_connected_error;
// socklen_t len = sizeof(error);
facilities_t *facilities = (facilities_t *) fc;
BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t));
facilities_t *facilities = (facilities_t *) fc; //roadRotationSin = sin(((config->applications.its_center.rotation + 90.0) * PI) / 180);
BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); //roadRotationCos = cos(((config->applications.its_center.rotation + 90.0) * PI) / 180);
//roadRotationSin = sin(((config->applications.its_center.rotation + 90.0) * PI) / 180);
//roadRotationCos = cos(((config->applications.its_center.rotation + 90.0) * PI) / 180);
roadRotationSin = 0.8660; // Default values for A3 Configuration roadRotationSin = 0.8660; // Default values for A3 Configuration
roadRotationCos = -0.5000; // Default values for A3 configuration roadRotationCos = -0.5000; // Default values for A3 configuration
/* Fill mandatory BTP Data Request parameters */ /* Fill mandatory BTP Data Request parameters */
bdr->gnDestinationAddress.buf = malloc(6); bdr->gnDestinationAddress.buf = malloc(6);
bdr->gnDestinationAddress.size = 6; bdr->gnDestinationAddress.size = 6;
for(int i = 0; i < 6; i++) for(int i = 0; i < 6; i++)
bdr->gnDestinationAddress.buf[i] = 0xff; //Broadcast addr bdr->gnDestinationAddress.buf[i] = 0xff; //Broadcast addr
bdr->btpType = BTPType_btpB; //BTP Type B is for non-interactive packet transport | BTP Type A is for interactive packet transport bdr->btpType = BTPType_btpB; //BTP Type B is for non-interactive packet transport | BTP Type A is for interactive packet transport
//The former doesn't have a source port and the latter have //The former doesn't have a source port and the latter have
bdr->destinationPort = Port_cpm; //CPM entity port for communication between Facilities and Transport bdr->destinationPort = Port_cpm; //CPM entity port for communication between Facilities and Transport
bdr->gnPacketTransportType = PacketTransportType_shb; //shb = Single Hop Broadcast packet bdr->gnPacketTransportType = PacketTransportType_shb; //shb = Single Hop Broadcast packet
bdr->gnTrafficClass = 2; //Identifier assigned to a GeoNetworking packet that expresses its requirements on data transport (WHY IS IT 2?) bdr->gnTrafficClass = 2; //Identifier assigned to a GeoNetworking packet that expresses its requirements on data transport (WHY IS IT 2?)
bdr->data.buf = malloc(1500); // CPM Data to be sent to the Transport layer bdr->data.buf = malloc(1500); // CPM Data to be sent to the Transport layer
if(facilities->use_security) { if(facilities->use_security) {
bdr->gnSecurityProfile = malloc(sizeof(long)); bdr->gnSecurityProfile = malloc(sizeof(long));
*bdr->gnSecurityProfile = 1; *bdr->gnSecurityProfile = 1;
} }
interface_connection(RADAR_IP,RADAR_PORT); // Create Radar listening socket is_radar_connected = interface_connection(RADAR_IP,RADAR_PORT); // Create Radar listening socket
struct timespec* systemtime; struct timespec* systemtime;
systemtime = (struct timespec*)malloc(sizeof(struct timespec)); systemtime = (struct timespec*)malloc(sizeof(struct timespec));
if (systemtime == NULL) { if (systemtime == NULL) {
printf("memory allocation failed: %m"); printf("memory allocation failed: %m");
exit(1); exit(1);
} }
while(!facilities->exit){ while(!facilities->exit){
usleep(1000*50); usleep(1000*50);
/* Receive Data from radar interface */
CPM_t* cpm_tx = calloc(1, sizeof(CPM_t));
i32_recv_bytes = recv(s_socket.i32_client, &au8_readBuffer, READ_BUFFER_SIZE, 0); //recv(socket,buffer,size,flags)
if (dissemination_check(facilities->dissemination) && facilities->dissemination->active){ if (dissemination_check(facilities->dissemination) && facilities->dissemination->active){
/* Receive Data from radar interface */ if(is_radar_connected){
CPM_t* cpm_tx = calloc(1, sizeof(CPM_t));
i32_recv_bytes = recv(s_socket.i32_client, &au8_readBuffer, READ_BUFFER_SIZE, 0); //recv(socket,buffer,size,flags)
if(i32_recv_bytes < 0){ if(i32_recv_bytes <= 0){
syslog_debug("No data received from radar ..."); syslog_debug("No data received from radar ...");
break; is_radar_connected = false;
} continue;
}
/* Information parsing */ /* Information parsing */
parse_input(au8_readBuffer,i32_recv_bytes); parse_input(au8_readBuffer,i32_recv_bytes);
/* CPM build and encoding (Could be merged if needed) */ /* CPM build and encoding (Could be merged if needed) */
mk_cpm(facilities,cpm_tx,systemtime); mk_cpm(facilities,cpm_tx,systemtime);
if(encode_cpm(cpm_tx, bdr->data.buf, (uint32_t *) &bdr->data.size) == 1){ if(encode_cpm(cpm_tx, bdr->data.buf, (uint32_t *) &bdr->data.size) == 1){
ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx);
continue; continue;
} }
/* Encode BTPDataRequest */ /* Encode BTPDataRequest */
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047); asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047);
if(enc.encoded == -1){ if(enc.encoded == -1){
syslog_err("[facilities] encoding BTPDataRequest for cpm failed"); syslog_err("[facilities] encoding BTPDataRequest for cpm failed");
continue; continue;
} }
/* Create thread to send packet to the Transport Layer (=3) */ /* Create thread to send packet to the Transport Layer (=3) */
queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3); queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3);
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); dissemination_reset_timer(facilities->dissemination);
/*TODO: Send message to applications to send it to the broker ? *FacilitiesDataIndication*/
/* TODO: Send message to applications to send it to the broker ? *FacilitiesDataIndication*/ }else{ /* Waiting for Radar to reconnect */
}
} is_radar_connected = waitingIncomingConnection();
}
}
}
ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr); ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr);
return NULL; return NULL;
} }