diff --git a/src/config.c b/src/config.c index 2f9b3f9..d421a03 100644 --- a/src/config.c +++ b/src/config.c @@ -250,7 +250,8 @@ int facilities_config(void* facilities_s) { facilities->dissemination->T_GenCpmMax = config->facilities.cpm.rsu_obu_period_max; facilities->dissemination->radar_rotation = config->applications.its_center.rotation; facilities->dissemination->tmc_connect = config->facilities.cpm.tmc_connected; - + facilities->dissemination->T_AddSensorInformation = 1000; + facilities->dissemination->int_radar = malloc(strlen(config->facilities.cpm.radar_interface)+1); strcpy(facilities->dissemination->int_radar,config->facilities.cpm.radar_interface); facilities->dissemination->ip_radar = malloc(strlen(config->facilities.cpm.radar_ip)+1); diff --git a/src/cpm.c b/src/cpm.c index 7b9b1fd..bdf10fd 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -34,11 +34,15 @@ #endif #define PI 3.141592654 +#define MAX_OBJ_RADAR 255 /* Variables */ float roadRotationSin; float roadRotationCos; +long history_list[MAX_OBJ_RADAR][5]; +int valid_array[MAX_OBJ_RADAR]; + S_ETHERNET_CONNECTION_T s_socket; S_INTERFACE_CONNECTION_T raw_socket; S_OBJECT_CONTROL_T s_objectControl; @@ -130,6 +134,67 @@ bool radar_connection(char* radar_port, facilities_t* facilities){ } +dissemination_t* dissemination_init(){ + /* Mutex init and dissemination memory allocation */ + dissemination_t* dissemination = (dissemination_t*) calloc(1, sizeof(dissemination_t)); + pthread_mutex_init(&dissemination->lock, NULL); + + return dissemination; +} + + +int dissemination_check(dissemination_t* dissemination, it2s_tender_epv_t* epv, int f) { + int rv = 1; + + uint64_t now = it2s_tender_get_clock(epv); + + pthread_mutex_lock(&dissemination->lock); // mutex is used to lock shared resources + + /* If f = 0 indicates that it is to check the Sensor Information Container timer + * If f = 1 inidcates that it is to check the CPM generation */ + + if(f == 0){ + if(now >= dissemination->next_AddSensorInformation){ + rv = 1; + }else{ + rv = 0; + } + }else{ + if (now >= dissemination->next_cpm_min){ + rv = 1; + }else if(now >= dissemination->next_cpm_max){ + rv = 0; + } + } + + pthread_mutex_unlock(&dissemination->lock); + + return rv; +} + + +void dissemination_reset_timer(dissemination_t* dissemination, it2s_tender_epv_t* epv, int f){ + + uint64_t now = it2s_tender_get_clock(epv); + + /* Both cases for RSU and OBU */ + + /* If f = 0 indicates that the reset corresponds to the timer of the Sensor Information Container Inclusion + If f = 1 indicates that the reset corresponds to the timer of the CPM generation */ + + pthread_mutex_lock(&dissemination->lock); + + if(f == 0){ + dissemination->next_AddSensorInformation = now + dissemination->T_AddSensorInformation; + }else{ + dissemination->next_cpm_min = now + dissemination->T_GenCpmMin; + dissemination->next_cpm_max = now + dissemination->T_GenCpmMax; + } + + pthread_mutex_unlock(&dissemination->lock); +} + + 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; @@ -403,13 +468,46 @@ 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){ + /* 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; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->timeOfMeasurement = 0; //Sem informaçao do radar + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->objectConfidence = 95; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xDistance.value = rotate_x( + (long)as_objects[i].f_xPoint * 100, (long)as_objects[i].f_yPoint * 100); + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xDistance.confidence = 102; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->yDistance.value = rotate_y( + (long)as_objects[i].f_xPoint * 100, (long)as_objects[i].f_yPoint * 100); + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->yDistance.confidence = 102; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xSpeed.value = rotate_x( + (long)as_objects[i].f_xSpeed * 100, (long)as_objects[i].f_ySpeed * 100); + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xSpeed.confidence = 40; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->ySpeed.value = rotate_y( + (long)as_objects[i].f_xSpeed * 100, (long)as_objects[i].f_ySpeed * 100); + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->ySpeed.confidence = 40; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->objectRefPoint = ObjectRefPoint_bottomMid; + + /* Fill History values */ + valid_array[(long)as_objects[i].u8_objectID] = 1; // Comparation Array + history_list[(long)as_objects[i].u8_objectID][0] = (long)as_objects[i].f_xPoint * 100; // xPoint (Distance) + history_list[(long)as_objects[i].u8_objectID][1] = (long)as_objects[i].f_yPoint * 100; // yPoint (Distance) + history_list[(long)as_objects[i].u8_objectID][2] = (long)as_objects[i].f_xSpeed * 100; // xSpeed (Speed) + history_list[(long)as_objects[i].u8_objectID][3] = (long)as_objects[i].f_ySpeed * 100; // ySpeed (Speed) + history_list[(long)as_objects[i].u8_objectID][4] = timestamp; // Time stamp of detected object +} + 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; - + + /* Variables */ CPM_t* cpm_tx = calloc(1, sizeof(CPM_t)); - + long euclidian_dist, abs_speed, abs_speed_hist, angle, angle_hist, angle_diff; + int j = 0, rv = 0; + int temp[MAX_OBJ_RADAR]; + + cpm_tx->header.protocolVersion = PROTOCOL_VERSION; cpm_tx->header.messageID = MESSAGE_ID; cpm_tx->header.stationID = facilities->id.value; @@ -426,60 +524,85 @@ static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len, cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeValue = 4000; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_alt_005_00; - cpm_tx->cpm.cpmParameters.sensorInformationContainer = calloc(1, sizeof(SensorInformationContainer_t)); - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.count = 1; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.size = 1; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array = calloc(1, sizeof(SensorInformation_t)); - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0] = calloc(1, sizeof(SensorInformation_t)); + if(dissemination_check(facilities->dissemination,&facilities->epv,0) == 1){ /* Sensor Information Container Inclusion Management */ - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->sensorID = 0; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->type = SensorType_radar; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.present = DetectionArea_PR_stationarySensorRadial; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.range = 3400; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleStart = ROAD_ANGLE * 10 - 500; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleEnd = ROAD_ANGLE * 10 + 500; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart = calloc(1, sizeof(CartesianAngleValue_t)); - (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart) = 1730; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd = calloc(1, sizeof(CartesianAngleValue_t)); - (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd) = 1890; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight = calloc(1, sizeof(SensorHeight_t)); - (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight) = 600; + cpm_tx->cpm.cpmParameters.sensorInformationContainer = calloc(1, sizeof(SensorInformationContainer_t)); + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.count = 1; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.size = 1; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array = calloc(1, sizeof(SensorInformation_t)); + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0] = calloc(1, sizeof(SensorInformation_t)); - cpm_tx->cpm.cpmParameters.perceivedObjectContainer = calloc(1, sizeof(PerceivedObjectContainer_t)); - - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = s_objectControl.u8_numberOfObjects; - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.size = s_objectControl.u8_numberOfObjects; - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array = calloc( - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count, - sizeof(PerceivedObject_t*)); - - - for (int i = 0; i < cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; i++) { - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i] = calloc(1, sizeof(PerceivedObject_t)); - - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->objectID = (long)as_objects[i].u8_objectID; - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->timeOfMeasurement = 0; //sem informaçao do radar - - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->objectConfidence = 95; - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->xDistance.value = rotate_x( - (long)as_objects[i].f_xPoint * 100, (long)as_objects[i].f_yPoint * 100); - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->xDistance.confidence = 102; - // printf("->%f<-\n", as_objects[i].f_xPoint); - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->yDistance.value = rotate_y( - (long)as_objects[i].f_xPoint * 100, (long)as_objects[i].f_yPoint * 100); - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->yDistance.confidence = 102; - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->xSpeed.value = rotate_x( - (long)as_objects[i].f_xSpeed * 100, (long)as_objects[i].f_ySpeed * 100); - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->xSpeed.confidence = 40; - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->ySpeed.value = rotate_y( - (long)as_objects[i].f_xSpeed * 100, (long)as_objects[i].f_ySpeed * 100); - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->ySpeed.confidence = 40; - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->objectRefPoint = ObjectRefPoint_bottomMid; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->sensorID = 0; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->type = SensorType_radar; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.present = DetectionArea_PR_stationarySensorRadial; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.range = 3400; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleStart = ROAD_ANGLE * 10 - 500; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleEnd = ROAD_ANGLE * 10 + 500; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart = calloc(1, sizeof(CartesianAngleValue_t)); + (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart) = 1730; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd = calloc(1, sizeof(CartesianAngleValue_t)); + (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd) = 1890; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight = calloc(1, sizeof(SensorHeight_t)); + (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight) = 600; + dissemination_reset_timer(facilities->dissemination, &facilities->epv, 0); } - cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer = calloc(1, sizeof(PerceivedObjectContainer_t)); + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array = calloc(s_objectControl.u8_numberOfObjects,sizeof(PerceivedObject_t*)); - /* Encode CPMs to FDI and BDR */ + 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)); + + 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); + j++; + + }else{ // The object is going to be compared (It was included in previous CPMs) (valid_array[id] = 1) + + // Getting the euclidian distance value from the object detected and the same object in the last cpm (xcurrent - xhistory)^2 + (ycurrent - yhistory)^2 + euclidian_dist = sqrt( pow(((long)as_objects[i].f_xPoint * 100) - (history_list[(long)as_objects[i].u8_objectID][0]), 2) + pow(((long)as_objects[i].f_yPoint * 100) - (history_list[(long)as_objects[i].u8_objectID][1]) ,2) ); + + // Getting the absolute speed value from the object detected and the same object in the last cpm (sqrt(x^2 + y^2)) + abs_speed = sqrt( pow( ((long)as_objects[i].f_xSpeed * 100),2) + pow( ( (long)as_objects[i].f_ySpeed * 100),2) ); + abs_speed_hist = sqrt( pow( history_list[(long)as_objects[i].u8_objectID][2] ,2) + pow( history_list[(long)as_objects[i].u8_objectID][3],2) ); // sqrt(xSpeed^2 + ySpeed^2) + + // Getting the angle from the velocity vector detected and the same object in the last cpm + angle = (long)((180 / PI) * atan2( (long)as_objects[i].f_ySpeed * 100 , (long)as_objects[i].f_xSpeed * 100 )); + angle_hist = (long)((180 / PI) * atan2( history_list[(long)as_objects[i].u8_objectID][3] , history_list[(long)as_objects[i].u8_objectID][2]) ); // tan(yspeed / xspeed) + angle_diff = ((angle - angle_hist) + 180) % 360 - 180; + + // 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){ + + // DEBUG messages + syslog_debug(" DEBUG : [Object ID : %d]",(long)as_objects[i].u8_objectID); + syslog_debug(" DEBUG : Distance traveled %ld", euclidian_dist ); + 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 : Time Diff %ld", abs(generationDeltaTime - history_list[i][4])); + syslog_debug(" DEBUG : \n\n\n"); + + set_values(i,j,generationDeltaTime,cpm_tx); + j++; + + }else{ //The object is not included but is valid for comparison in the upcoming CPMs + valid_array[(long)as_objects[i].u8_objectID] = 1; + } + } + } + + + cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; // Object perceived by the Radar (Does not have to match the objects included in the CPM) + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = j; // The number of objects that were included in the CPM + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.size = j; + cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = j; + + /******* Encode CPMs to FDI and BDR ********/ //BDR memset(bdr_oer, 0, 1500); @@ -509,60 +632,14 @@ cleanup: return rv; } -dissemination_t* dissemination_init(){ - /* Mutex init and dissemination memory allocation */ - dissemination_t* dissemination = (dissemination_t*) calloc(1, sizeof(dissemination_t)); - pthread_mutex_init(&dissemination->lock, NULL); - - return dissemination; -} - - -int dissemination_check(dissemination_t* dissemination, it2s_tender_epv_t* epv) { - int rv = 0; - - uint64_t now = it2s_tender_get_clock(epv); - - /* mutex is used to lock shared resources */ - - pthread_mutex_lock(&dissemination->lock); - - // Both cases for OBU and RSU (BASIC: generation interval 1s) - if (now >= dissemination->next_cpm_min){ - rv = 1; - }else if(now >= dissemination->next_cpm_max){ - rv = 0; - } - - pthread_mutex_unlock(&dissemination->lock); - - return rv; -} - - -void dissemination_reset_timer(dissemination_t* dissemination, it2s_tender_epv_t* epv){ - - uint64_t now = it2s_tender_get_clock(epv); - - /* Both cases for RSU and OBU */ - - pthread_mutex_lock(&dissemination->lock); - - dissemination->next_cpm_min = now + dissemination->T_GenCpmMin; - dissemination->next_cpm_max = now + dissemination->T_GenCpmMax; - - pthread_mutex_unlock(&dissemination->lock); -} - - void *cp_service(void *fc){ /* Variables */ - int i32_recv_bytes; u_int8_t au8_readBuffer[READ_BUFFER_SIZE]; bool is_radar_connected; u_int8_t au8_readTcp[READ_BUFFER_SIZE]; facilities_t *facilities = (facilities_t *) fc; + memset(valid_array, 0, sizeof(valid_array)); uint8_t bdr_oer[2048]; uint8_t fdi_oer[2048]; @@ -616,15 +693,14 @@ void *cp_service(void *fc){ /* Reads from the radar */ 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 (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 */ parse_input(au8_readBuffer,i32_recv_bytes); @@ -656,7 +732,7 @@ void *cp_service(void *fc){ /*Reset Timer for dissemination control */ - dissemination_reset_timer(facilities->dissemination, &facilities->epv); + dissemination_reset_timer(facilities->dissemination, &facilities->epv,1); } else { /* Waiting for Radar to reconnect */ diff --git a/src/cpm.h b/src/cpm.h index f124d2a..ac4a050 100644 --- a/src/cpm.h +++ b/src/cpm.h @@ -131,6 +131,9 @@ typedef struct uint64_t T_GenCpmMin; uint64_t T_GenCpmMax; + uint64_t T_AddSensorInformation; + uint64_t next_AddSensorInformation; + /* Position of the radar (Value from toml) */