TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'master' of https://gitlab.es.av.it.pt/its/it2s-itss-facilities

This commit is contained in:
emanuel 2021-07-05 18:27:57 +01:00
parent c75624d8ca 4e072faf67
commit 268e245679
3 changed files with 209 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/config.c
View File

@ -250,6 +250,7 @@ int facilities_config(void* facilities_s) {
facilities->dissemination->T_GenCpmMax = config->facilities.cpm.rsu_obu_period_max; facilities->dissemination->T_GenCpmMax = config->facilities.cpm.rsu_obu_period_max;
facilities->dissemination->radar_rotation = config->applications.its_center.rotation; facilities->dissemination->radar_rotation = config->applications.its_center.rotation;
facilities->dissemination->tmc_connect = config->facilities.cpm.tmc_connected; 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); facilities->dissemination->int_radar = malloc(strlen(config->facilities.cpm.radar_interface)+1);
strcpy(facilities->dissemination->int_radar,config->facilities.cpm.radar_interface); strcpy(facilities->dissemination->int_radar,config->facilities.cpm.radar_interface);

345
src/cpm.c
View File

@ -34,38 +34,21 @@
#endif #endif
#define PI 3.141592654 #define PI 3.141592654
#define MAX_OBJ_RADAR 255
/* Variables */ /* Variables */
float roadRotationSin; float roadRotationSin;
float roadRotationCos; float roadRotationCos;
long history_list[MAX_OBJ_RADAR][5];
int valid_array[MAX_OBJ_RADAR];
S_ETHERNET_CONNECTION_T s_socket; S_ETHERNET_CONNECTION_T s_socket;
S_INTERFACE_CONNECTION_T raw_socket; S_INTERFACE_CONNECTION_T raw_socket;
S_OBJECT_CONTROL_T s_objectControl; S_OBJECT_CONTROL_T s_objectControl;
S_OBJECTS_T as_objects[NOF_OBJECTS]; 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){ bool radar_connection(char* radar_port, facilities_t* facilities){
if(facilities->dissemination->tmc_connect == false){ if(facilities->dissemination->tmc_connect == false){
@ -84,15 +67,27 @@ 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_addr.s_addr = inet_addr(facilities->dissemination->ip_radar);
s_socket.s_server.sin_port = htons(atoi(radar_port)); 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 ..."); syslog_err("Binding socket to address error ...");
}
// 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; return false;
} }
if(!waitingIncomingConnection()) if((s_socket.i32_client = accept(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server, &len)) < 0){
syslog_err("Client disconnected...");
return false; return false;
} }
syslog_debug("Radar connected");
}
// Create RAW socket // Create RAW socket
raw_socket.raw_fd = socket(AF_PACKET, SOCK_RAW, htons(0x0800)); raw_socket.raw_fd = socket(AF_PACKET, SOCK_RAW, htons(0x0800));
@ -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) { 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 u8_pvrMessagePart = 0;
u_int8_t tmp = 0; u_int8_t tmp = 0;
@ -403,12 +459,45 @@ long rotate_y(long x, long y) {
return (long)(x * roadRotationSin + y * roadRotationCos); 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) { 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)); 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.protocolVersion = PROTOCOL_VERSION;
cpm_tx->header.messageID = MESSAGE_ID; cpm_tx->header.messageID = MESSAGE_ID;
@ -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.altitudeValue = 4000;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_alt_005_00; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_alt_005_00;
cpm_tx->cpm.cpmParameters.sensorInformationContainer = calloc(1, sizeof(SensorInformationContainer_t)); if(dissemination_check(facilities->dissemination,&facilities->epv,0) == 1){ /* Sensor Information Container Inclusion Management */
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.sensorInformationContainer->list.array[0]->sensorID = 0; cpm_tx->cpm.cpmParameters.sensorInformationContainer = calloc(1, sizeof(SensorInformationContainer_t));
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->type = SensorType_radar; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.count = 1;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.present = DetectionArea_PR_stationarySensorRadial; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.size = 1;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.range = 3400; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array = calloc(1, sizeof(SensorInformation_t));
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleStart = ROAD_ANGLE * 10 - 500; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0] = calloc(1, sizeof(SensorInformation_t));
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.perceivedObjectContainer = calloc(1, sizeof(PerceivedObjectContainer_t)); 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.perceivedObjectContainer->list.count = s_objectControl.u8_numberOfObjects; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.present = DetectionArea_PR_stationarySensorRadial;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.size = s_objectControl.u8_numberOfObjects; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.range = 3400;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array = calloc( cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleStart = ROAD_ANGLE * 10 - 500;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count, cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleEnd = ROAD_ANGLE * 10 + 500;
sizeof(PerceivedObject_t*)); 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));
for (int i = 0; i < cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; i++) { (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd) = 1890;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i] = calloc(1, sizeof(PerceivedObject_t)); 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.perceivedObjectContainer->list.array[i]->objectID = (long)as_objects[i].u8_objectID; dissemination_reset_timer(facilities->dissemination, &facilities->epv, 0);
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.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 //BDR
memset(bdr_oer, 0, 1500); memset(bdr_oer, 0, 1500);
@ -509,60 +625,13 @@ cleanup:
return rv; 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){ void *cp_service(void *fc){
/* Variables */ /* Variables */
int i32_recv_bytes; int i32_recv_bytes;
u_int8_t au8_readBuffer[READ_BUFFER_SIZE]; u_int8_t au8_readBuffer[READ_BUFFER_SIZE];
bool is_radar_connected;
u_int8_t au8_readTcp[READ_BUFFER_SIZE]; u_int8_t au8_readTcp[READ_BUFFER_SIZE];
facilities_t *facilities = (facilities_t *) fc; facilities_t *facilities = (facilities_t *) fc;
memset(valid_array, 0, sizeof(valid_array));
uint8_t bdr_oer[2048]; uint8_t bdr_oer[2048];
uint8_t fdi_oer[2048]; uint8_t fdi_oer[2048];
@ -601,29 +670,21 @@ void *cp_service(void *fc){
fdi->data.buf = malloc(1500); fdi->data.buf = malloc(1500);
/* Creating sockets and waiting for radar to connect*/ /* Creating sockets and waiting for radar to connect*/
is_radar_connected = radar_connection(RADAR_PORT,facilities); radar_connection(RADAR_PORT,facilities);
while(!facilities->exit){ while(!facilities->exit){
usleep(1000*50); 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 */ /* Reads from the radar */
/* To maintain the connection the content must be read */ i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0);
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);
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 */ /* Information parsing from radar */
parse_input(au8_readBuffer,i32_recv_bytes); parse_input(au8_readBuffer,i32_recv_bytes);
@ -655,14 +716,8 @@ void *cp_service(void *fc){
pthread_cond_signal(&facilities->tx_queue->trigger); pthread_cond_signal(&facilities->tx_queue->trigger);
/*Reset Timer for dissemination control */ /*Reset Timer for dissemination control */
dissemination_reset_timer(facilities->dissemination, &facilities->epv,1);
dissemination_reset_timer(facilities->dissemination, &facilities->epv); }
} else { /* Waiting for Radar to reconnect */
is_radar_connected = waitingIncomingConnection();
}
}
} }
ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr); ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr);

3
src/cpm.h
View File

@ -131,6 +131,9 @@ typedef struct
uint64_t T_GenCpmMin; uint64_t T_GenCpmMin;
uint64_t T_GenCpmMax; uint64_t T_GenCpmMax;
uint64_t T_AddSensorInformation;
uint64_t next_AddSensorInformation;
/* Position of the radar (Value from toml) */ /* Position of the radar (Value from toml) */