From 30397c9fb8520fabf2d6826904c70f7b11c4d782 Mon Sep 17 00:00:00 2001 From: Marco Correia Date: Mon, 27 Dec 2021 15:46:56 +0000 Subject: [PATCH] Moved global variables into the service --- src/cpm.c | 37 ++++++++++++++++++++----------------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/src/cpm.c b/src/cpm.c index 2eea5c5..44c063e 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -39,8 +39,6 @@ float roadRotationSin; float roadRotationCos; -long history_list[NOF_OBJECTS][5]; -int valid_array[NOF_OBJECTS]; S_ETHERNET_CONNECTION_T s_socket; S_INTERFACE_CONNECTION_T raw_socket; @@ -356,7 +354,7 @@ void parse_can_data_tm(u_int32_t u32_can_id, int i32_can_len, u_int8_t* au8_can_ void parse_input(u_int8_t* u8_input_buffer, int i32_len) { - enum state_t s_state = IDLE; + static enum state_t s_state = IDLE; u_int8_t u8_input = 0; int i32_i; @@ -481,7 +479,7 @@ long rotate_y(long x, long y) { return (long)(x * roadRotationSin + y * roadRotationCos); } -void set_values(int i, int j, uint64_t timestamp, CPM_t* cpm_tx){ +void set_values(int i, int j, uint64_t timestamp, CPM_t* cpm_tx, long history_list[NOF_OBJECTS][5], int valid_array[]){ /* Fill CPM */ cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j] = calloc(1, sizeof(PerceivedObject_t)); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->objectID = (long)as_objects[i].u8_objectID; @@ -511,7 +509,7 @@ void set_values(int i, int j, uint64_t timestamp, CPM_t* cpm_tx){ } -static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len, uint8_t *fdi_oer, uint32_t *fdi_len) { +static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len, uint8_t *fdi_oer, uint32_t *fdi_len, long history_list[NOF_OBJECTS][5], int valid_array[]) { /* Variables */ CPM_t* cpm_tx = calloc(1, sizeof(CPM_t)); @@ -576,13 +574,13 @@ static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len, cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = s_objectControl.u8_numberOfObjects; - memcpy(temp, valid_array, sizeof(valid_array)); - memset(valid_array, 0, sizeof(valid_array)); + memcpy(temp, valid_array, NOF_OBJECTS * sizeof(int)); // NOF_OBJECTS * sizeof(int) = size of valid_array + memset(valid_array, 0, NOF_OBJECTS * sizeof(int)); for(int i = 0; i < cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count;i++){ if(temp[(long)as_objects[i].u8_objectID] == 0 ){ // The object is going to be added without comparison (It is a new object) (valid_array[id] = 0) - set_values(i,j,generationDeltaTime,cpm_tx); + set_values(i,j,generationDeltaTime,cpm_tx,history_list,valid_array); j++; }else{ // The object is going to be compared (It was included in previous CPMs) (valid_array[id] = 1) @@ -602,7 +600,7 @@ 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){ - set_values(i,j,generationDeltaTime,cpm_tx); + set_values(i,j,generationDeltaTime,cpm_tx, history_list, valid_array); j++; }else{ //The object is not included but is valid for comparison in the upcoming CPMs @@ -654,7 +652,11 @@ void *cp_service(void *fc){ u_int8_t au8_readBuffer[READ_BUFFER_SIZE]; u_int8_t au8_readTcp[READ_BUFFER_SIZE]; bool is_radar_connected; + long history_list[NOF_OBJECTS][5]; + int valid_array[NOF_OBJECTS]; + facilities_t *facilities = (facilities_t *) fc; + memset(history_list, 0, sizeof(history_list)); memset(valid_array, 0, sizeof(valid_array)); uint8_t tr_oer[2048]; @@ -662,18 +664,18 @@ void *cp_service(void *fc){ tr_oer[0] = 4; //Facilities fi_oer[0] = 4; - TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t)); - tr->present = TransportRequest_PR_packet; - TransportPacketRequest_t* tpr = &tr->choice.packet; - tpr->present = TransportPacketRequest_PR_btp; + TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t)); + tr->present = TransportRequest_PR_packet; + TransportPacketRequest_t* tpr = &tr->choice.packet; + tpr->present = TransportPacketRequest_PR_btp; BTPPacketRequest_t *bpr = &tpr->choice.btp; FacilitiesIndication_t* fi = calloc(1, sizeof(FacilitiesIndication_t)); - fi->present = FacilitiesIndication_PR_message; - FacilitiesMessageIndication_t* fmi = &fi->choice.message; + fi->present = FacilitiesIndication_PR_message; + FacilitiesMessageIndication_t* fmi = &fi->choice.message; roadRotationSin = sin(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180); - roadRotationCos = cos(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180); + roadRotationCos = cos(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180); /*--- Fill mandatory BTP Data Request parameters ---*/ bpr->gn.destinationAddress.buf = malloc(6); @@ -710,6 +712,7 @@ void *cp_service(void *fc){ /* Reads from the radar */ i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0); + syslog_debug("DEBUG : %d", i32_recv_bytes); if (dissemination_check(facilities->dissemination, &facilities->epv,1) && facilities->dissemination->active){ if(is_radar_connected){ @@ -717,7 +720,7 @@ void *cp_service(void *fc){ parse_input(au8_readBuffer,i32_recv_bytes); /* CPM build and encoding to BDR and FDI */ - if(mk_cpm(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size) == 1) + if(mk_cpm(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size, history_list, valid_array) == 1) continue; /* Encode TransportRequest */