From ae5701bb87ff6488431fc8bb9d8bff9a7e1baa4a Mon Sep 17 00:00:00 2001 From: Marco Date: Fri, 23 Apr 2021 15:47:39 +0100 Subject: [PATCH] Added the ability for radar to reconnect (CPM) --- src/cpm.c | 175 +++++++++++++++++++++++++++++------------------------- 1 file changed, 93 insertions(+), 82 deletions(-) diff --git a/src/cpm.c b/src/cpm.c index 36ea93f..e19c6a6 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -11,6 +11,7 @@ #include #include + #include #include #include @@ -41,6 +42,26 @@ char* station_id; 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){ @@ -63,26 +84,14 @@ bool interface_connection(char* radar_ip,char* radar_port){ return false; } - - // 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..."); + if(!waitingIncomingConnection()) 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; -} +} + 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 tmp = 0; @@ -511,107 +520,109 @@ void dissemination_reset_timer(dissemination_t* dissemination){ void *cp_service(void *fc){ - /* Variables */ - int i32_recv_bytes; - u_int8_t au8_readBuffer[READ_BUFFER_SIZE]; - //void* message_data = malloc(1500); - uint8_t bdr_oer[2048]; - bdr_oer[0] = 4; //Facilities - int is_radar_connected_error; - // socklen_t len = sizeof(error); + /* Variables */ + int i32_recv_bytes; + u_int8_t au8_readBuffer[READ_BUFFER_SIZE]; + uint8_t bdr_oer[2048]; + bdr_oer[0] = 4; //Facilities + bool is_radar_connected; + + facilities_t *facilities = (facilities_t *) fc; + BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); - facilities_t *facilities = (facilities_t *) fc; - BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); - - //roadRotationSin = sin(((config->applications.its_center.rotation + 90.0) * PI) / 180); - //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 - /* Fill mandatory BTP Data Request parameters */ + /* Fill mandatory BTP Data Request parameters */ - bdr->gnDestinationAddress.buf = malloc(6); - bdr->gnDestinationAddress.size = 6; - for(int i = 0; i < 6; i++) - bdr->gnDestinationAddress.buf[i] = 0xff; //Broadcast addr + bdr->gnDestinationAddress.buf = malloc(6); + bdr->gnDestinationAddress.size = 6; + for(int i = 0; i < 6; i++) + 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 - bdr->destinationPort = Port_cpm; //CPM entity port for communication between Facilities and Transport - 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->data.buf = malloc(1500); // CPM Data to be sent to the Transport layer + bdr->destinationPort = Port_cpm; //CPM entity port for communication between Facilities and Transport + 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->data.buf = malloc(1500); // CPM Data to be sent to the Transport layer - if(facilities->use_security) { - bdr->gnSecurityProfile = malloc(sizeof(long)); - *bdr->gnSecurityProfile = 1; - } + if(facilities->use_security) { + bdr->gnSecurityProfile = malloc(sizeof(long)); + *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; systemtime = (struct timespec*)malloc(sizeof(struct timespec)); - if (systemtime == NULL) { - printf("memory allocation failed: %m"); - exit(1); - } + if (systemtime == NULL) { + printf("memory allocation failed: %m"); + exit(1); + } - while(!facilities->exit){ 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){ - /* 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(is_radar_connected){ - if(i32_recv_bytes < 0){ - syslog_debug("No data received from radar ..."); - break; - } + if(i32_recv_bytes <= 0){ + syslog_debug("No data received from radar ..."); + is_radar_connected = false; + continue; + } - /* Information parsing */ - parse_input(au8_readBuffer,i32_recv_bytes); + /* Information parsing */ + parse_input(au8_readBuffer,i32_recv_bytes); - /* CPM build and encoding (Could be merged if needed) */ - mk_cpm(facilities,cpm_tx,systemtime); + /* CPM build and encoding (Could be merged if needed) */ + mk_cpm(facilities,cpm_tx,systemtime); - if(encode_cpm(cpm_tx, bdr->data.buf, (uint32_t *) &bdr->data.size) == 1){ - ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); - continue; - } + if(encode_cpm(cpm_tx, bdr->data.buf, (uint32_t *) &bdr->data.size) == 1){ + ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); + continue; + } - /* Encode BTPDataRequest */ + /* Encode BTPDataRequest */ - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047); - if(enc.encoded == -1){ - syslog_err("[facilities] encoding BTPDataRequest for cpm failed"); - continue; - } + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047); + if(enc.encoded == -1){ + syslog_err("[facilities] encoding BTPDataRequest for cpm failed"); + continue; + } - /* Create thread to send packet to the Transport Layer (=3) */ - queue_add(facilities->tx_queue, bdr_oer, enc.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, bdr_oer, enc.encoded+1, 3); + 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); return NULL; - }