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

Merge remote-tracking branch 'refs/remotes/origin/master'

This commit is contained in:
emanuel 2022-04-13 17:27:11 +01:00
parent fb3539104e 85f0cfe04b
commit 79ab400337
4 changed files with 48 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
src/cpm.c
View File

@ -560,49 +560,51 @@ static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len,
dissemination_reset_timer(facilities->dissemination, &facilities->epv, 0);
}
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*));
if (s_objectControl.u8_numberOfObjects > 0) {
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*));
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = s_objectControl.u8_numberOfObjects;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = s_objectControl.u8_numberOfObjects;
memcpy(temp, valid_array, NOF_OBJECTS * sizeof(int)); // NOF_OBJECTS * sizeof(int) = size of valid_array
memset(valid_array, 0, NOF_OBJECTS * sizeof(int));
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[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,history_list,valid_array,history_timestamp);
j++;
for(int i = 0; i < cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count;i++){
if(temp[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,history_list,valid_array,history_timestamp);
j++;
}else{ // The object is going to be compared (It was included in previous CPMs) (valid_array[id] = 1)
}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 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 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;
// 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)
// 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_timestamp[i]) >= facilities->dissemination->T_GenCpmMax){
set_values(i,j,generationDeltaTime,cpm_tx, history_list, valid_array,history_timestamp);
j++;
if(abs(euclidian_dist) > 400 || abs(abs_speed - abs_speed_hist) > 50 || abs(angle_diff) > 4 || abs(generationDeltaTime - history_timestamp[i]) >= facilities->dissemination->T_GenCpmMax){
set_values(i,j,generationDeltaTime,cpm_tx, history_list, valid_array,history_timestamp);
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;
}
}
}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 = 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;

8
src/facilities.c
View File

@ -225,11 +225,15 @@ static int transport_indication(facilities_t *facilities, void* responder, void*
break;
case 7011:
tpm_recv(facilities, its_msg, neighbour_cert);
if (facilities->tolling.active) {
tpm_recv(facilities, its_msg, neighbour_cert);
}
break;
case 2043:
pcm_check(facilities, its_msg);
if (facilities->coordination.active) {
pcm_check(facilities, its_msg);
}
fwd = true;
break;

9
src/pcm.c
View File

@ -35,7 +35,7 @@ static int are_vehicles_intersecting(
// B2[1] = tB[b+1].longitude;
// if (it2s_tender_do_segments_intersect(A1, A2, B1, B2)) {
if (it2s_tender_is_inside_rectangle(tA[a].latitude, tA[a].longitude, tB[b].latitude, tB[b].longitude, 8, 8, 0, DCM_HAVERSINE)) {
if (it2s_tender_is_inside_circle(tA[a].latitude, tA[a].longitude, tB[b].latitude, tB[b].longitude, 8, DCM_HAVERSINE)) {
if (tA[a].timestamp < tB[b].timestamp + 2000 &&
tA[a].timestamp > tB[b].timestamp - 2000) {
*index = a;
@ -219,7 +219,12 @@ static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm, mc_neigh
uint64_t now_us = it2s_tender_get_now(TIME_MICROSECONDS);
pthread_mutex_unlock(&facilities->epv.time.lock);
syslog_info("[facilities] [pc] received PCM.reply with %d accepted trajectories | took %ld us", reply->acceptedTrajectoriesIds.list.count, now_us-neighbour->t_iid);
if (neighbour->intersecting) {
syslog_info("[facilities] [pc] received PCM.reply with %d accepted trajectories | took %ld us", reply->acceptedTrajectoriesIds.list.count, now_us-neighbour->t_iid);
} else {
syslog_info("[facilities] [pc] received PCM.reply is response to another ITS-S PCM.request");
}
neighbour->intersecting = false;
neighbour->proposed = false;

2
src/saem.c
View File

@ -244,7 +244,7 @@ int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) {
exts->list.array[3]->present = ServiceInfoExt_PR_applicationDataSAM;
exts->list.array[3]->choice.applicationDataSAM.buf = malloc(1024);
enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities->tolling.infos.z[0], exts->list.array[3]->choice.applicationDataSAM.buf, 1024);
enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities->tolling.infos.z[0]->asn, exts->list.array[3]->choice.applicationDataSAM.buf, 1024);
if (enc.encoded == -1) {
syslog_err("[facilities] [sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name);
rv = 1;