Merge branch 'master' of https://gitlab.es.av.it.pt/its/it2s-itss-facilities
This commit is contained in:
emanuel
commit
268e245679
|
|
@ -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);
|
||||
|
|
|
|||
353
src/cpm.c
353
src/cpm.c
|
|
@ -34,38 +34,21 @@
|
|||
#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;
|
||||
S_OBJECTS_T as_objects[NOF_OBJECTS];
|
||||
|
||||
|
||||
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 radar_connection(char* radar_port, facilities_t* facilities){
|
||||
|
||||
if(facilities->dissemination->tmc_connect == false){
|
||||
|
|
@ -84,14 +67,26 @@ bool radar_connection(char* radar_port, facilities_t* facilities){
|
|||
s_socket.s_server.sin_addr.s_addr = inet_addr(facilities->dissemination->ip_radar);
|
||||
s_socket.s_server.sin_port = htons(atoi(radar_port));
|
||||
|
||||
if(bind(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server,sizeof(s_socket.s_server)) < 0){
|
||||
while(bind(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server,sizeof(s_socket.s_server)) < 0){
|
||||
syslog_err("Binding socket to address error ...");
|
||||
return false;
|
||||
}
|
||||
|
||||
if(!waitingIncomingConnection())
|
||||
// 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");
|
||||
|
||||
}
|
||||
|
||||
// Create RAW socket
|
||||
|
||||
|
|
@ -130,6 +125,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 +459,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 +515,87 @@ 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;
|
||||
|
||||
|
||||
//syslog_debug( " DEBUG -------------------------------------------------");
|
||||
/******* Encode CPMs to FDI and BDR ********/
|
||||
|
||||
//BDR
|
||||
memset(bdr_oer, 0, 1500);
|
||||
|
|
@ -509,60 +625,13 @@ 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];
|
||||
|
|
@ -601,30 +670,22 @@ void *cp_service(void *fc){
|
|||
fdi->data.buf = malloc(1500);
|
||||
|
||||
/* Creating sockets and waiting for radar to connect*/
|
||||
is_radar_connected = radar_connection(RADAR_PORT,facilities);
|
||||
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);
|
||||
|
||||
/* 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 */
|
||||
i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0);
|
||||
|
||||
/* 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(is_radar_connected){
|
||||
if(i32_recv_bytes <= 0){
|
||||
syslog_debug("No data received from radar ...");
|
||||
is_radar_connected = false;
|
||||
continue;
|
||||
}
|
||||
if (dissemination_check(facilities->dissemination, &facilities->epv,1) && facilities->dissemination->active){
|
||||
|
||||
|
||||
/* Information parsing from radar */
|
||||
parse_input(au8_readBuffer,i32_recv_bytes);
|
||||
|
||||
|
|
@ -654,15 +715,9 @@ void *cp_service(void *fc){
|
|||
queue_add(facilities->tx_queue, fdi_oer, enc_fdi.encoded+1, 5);
|
||||
pthread_cond_signal(&facilities->tx_queue->trigger);
|
||||
|
||||
/*Reset Timer for dissemination control */
|
||||
|
||||
dissemination_reset_timer(facilities->dissemination, &facilities->epv);
|
||||
|
||||
|
||||
} else { /* Waiting for Radar to reconnect */
|
||||
is_radar_connected = waitingIncomingConnection();
|
||||
}
|
||||
}
|
||||
/*Reset Timer for dissemination control */
|
||||
dissemination_reset_timer(facilities->dissemination, &facilities->epv,1);
|
||||
}
|
||||
}
|
||||
|
||||
ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr);
|
||||
|
|
|
|||
Loading…
Reference in New Issue