From 91792d00e1ce6cbb9058695b5968f9ffbcd25b27 Mon Sep 17 00:00:00 2001 From: Marco Correia Date: Mon, 7 Jun 2021 16:34:05 +0100 Subject: [PATCH] Merged encode_cpm routine into mk_cpm. Deleted unnecessary variables --- src/cpm.c | 116 +++++++++++++++++++++++++++--------------------------- 1 file changed, 57 insertions(+), 59 deletions(-) diff --git a/src/cpm.c b/src/cpm.c index 78933ca..7b9b1fd 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -66,7 +66,7 @@ bool waitingIncomingConnection(void){ -bool interface_connection(char* radar_port, facilities_t* facilities){ +bool radar_connection(char* radar_port, facilities_t* facilities){ if(facilities->dissemination->tmc_connect == false){ @@ -404,9 +404,12 @@ long rotate_y(long x, long y) { } -void mk_cpm(facilities_t* facilities,CPM_t* cpm_tx, struct timespec* systemtime) { - +static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len, uint8_t *fdi_oer, uint32_t *fdi_len) { + int rv = 0; + CPM_t* cpm_tx = calloc(1, sizeof(CPM_t)); + + cpm_tx->header.protocolVersion = PROTOCOL_VERSION; cpm_tx->header.messageID = MESSAGE_ID; cpm_tx->header.stationID = facilities->id.value; @@ -475,21 +478,35 @@ void mk_cpm(facilities_t* facilities,CPM_t* cpm_tx, struct timespec* systemtime) } cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; -} - -int encode_cpm(CPM_t* cpm_tx, uint8_t *out_message_data, uint32_t *out_message_size) { - - memset(out_message_data, 0, 1500); - asn_enc_rval_t retval_enc = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, out_message_data, 1500); - if (retval_enc.encoded == -1) { - syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc.failed_type->name); - return 1; + /* Encode CPMs to FDI and BDR */ + + //BDR + memset(bdr_oer, 0, 1500); + asn_enc_rval_t retval_enc_bdr = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, bdr_oer, 1500); + if (retval_enc_bdr.encoded == -1) { + syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc_bdr.failed_type->name); + rv = 1; + goto cleanup; } - *out_message_size = ((retval_enc.encoded + 7) / 8); + *bdr_len = ((retval_enc_bdr.encoded + 7) / 8); - return 0; + //FDI + memset(fdi_oer, 0, 1500); + asn_enc_rval_t retval_enc_fdi = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, fdi_oer, 1500); + if (retval_enc_fdi.encoded == -1) { + syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc_fdi.failed_type->name); + rv = 1; + goto cleanup; + } + + *fdi_len = ((retval_enc_fdi.encoded + 7) / 8); + +cleanup: + ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); + + return rv; } dissemination_t* dissemination_init(){ @@ -511,9 +528,11 @@ int dissemination_check(dissemination_t* dissemination, it2s_tender_epv_t* epv) pthread_mutex_lock(&dissemination->lock); // Both cases for OBU and RSU (BASIC: generation interval 1s) - if (now >= dissemination->next_cpm_max) + if (now >= dissemination->next_cpm_min){ rv = 1; - + }else if(now >= dissemination->next_cpm_max){ + rv = 0; + } pthread_mutex_unlock(&dissemination->lock); @@ -536,8 +555,7 @@ void dissemination_reset_timer(dissemination_t* dissemination, it2s_tender_epv_t } -void *cp_service(void *fc){ - +void *cp_service(void *fc){ /* Variables */ int i32_recv_bytes; @@ -548,7 +566,7 @@ void *cp_service(void *fc){ uint8_t bdr_oer[2048]; uint8_t fdi_oer[2048]; - bdr_oer[0] = 4; //Facilities + bdr_oer[0] = 4; //Facilities fdi_oer[0] = 4; BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); @@ -558,7 +576,6 @@ void *cp_service(void *fc){ roadRotationCos = cos(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180); /*--- Fill mandatory BTP Data Request parameters ---*/ - bdr->gnDestinationAddress.buf = malloc(6); bdr->gnDestinationAddress.size = 6; for(int i = 0; i < 6; i++) @@ -579,39 +596,28 @@ void *cp_service(void *fc){ *bdr->gnSecurityProfile = 1; } - is_radar_connected = interface_connection(RADAR_PORT,facilities); // Create Radar listening socket - - /*--- Fill mandatory Facilities Data Indication parameters ---*/ - fdi->itsMessageType = ItsMessageType_cpm; fdi->data.buf = malloc(1500); - - struct timespec* systemtime; - systemtime = (struct timespec*)malloc(sizeof(struct timespec)); - if (systemtime == NULL) { - printf("memory allocation failed: %m"); - exit(1); - } - + /* Creating sockets and waiting for radar to connect*/ + is_radar_connected = radar_connection(RADAR_PORT,facilities); + + while(!facilities->exit){ 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); /* Reads from the radar */ - CPM_t* cpm_tx = calloc(1, sizeof(CPM_t)); - i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0); //recv(socket,buffer,size,flags) - + i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0); if (dissemination_check(facilities->dissemination, &facilities->epv) && facilities->dissemination->active){ - if(is_radar_connected){ - - + if(is_radar_connected){ if(i32_recv_bytes <= 0){ syslog_debug("No data received from radar ..."); is_radar_connected = false; @@ -619,27 +625,14 @@ void *cp_service(void *fc){ } - /* Information parsing */ + /* Information parsing from radar */ parse_input(au8_readBuffer,i32_recv_bytes); - /* CPM build and encoding to bdr and fdi (Could be merged if needed) */ - mk_cpm(facilities,cpm_tx,systemtime); + /* CPM build and encoding to BDR and FDI */ + if(mk_cpm(facilities, bdr->data.buf, (uint32_t *) &bdr->data.size, fdi->data.buf, (uint32_t *) &fdi->data.size) == 1) + continue; - - 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, fdi->data.buf, (uint32_t *) &fdi->data.size) == 1){ - ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); - continue; - } - - ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); - /* Encode BTPDataRequest */ - asn_enc_rval_t enc_bdr = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047); if(enc_bdr.encoded == -1){ syslog_err("[facilities] encoding BTPDataRequest for cpm failed"); @@ -647,7 +640,6 @@ void *cp_service(void *fc){ } /* Encode FacilitiesDataIndication */ - asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesDataIndication, NULL, fdi, fdi_oer+1, 2047); if(enc_fdi.encoded == -1){ syslog_err("[facilities] encoding FacilitiesDataIndication for cpm failed"); @@ -675,5 +667,11 @@ void *cp_service(void *fc){ ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr); ASN_STRUCT_FREE(asn_DEF_FacilitiesDataIndication,fdi); - return NULL; + + /* Close sockets */ + if(facilities->dissemination->tmc_connect) + shutdown(s_socket.i32_socket,2); + + shutdown(raw_socket.raw_fd,2); + return NULL; }