From 3b58068f785e952029193c7f2b8081b007b1183f Mon Sep 17 00:00:00 2001 From: Mohannad Jooriah Date: Fri, 8 Apr 2022 16:36:07 +0100 Subject: [PATCH 1/7] remove perceivedObjectContainer from the CPM when no objects are detected --- src/cpm.c | 66 ++++++++++++++++++++++++++++--------------------------- 1 file changed, 34 insertions(+), 32 deletions(-) diff --git a/src/cpm.c b/src/cpm.c index af225db..649ccc4 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -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; From 47c0e6fc46957498a1c3baa6c6b8da2ed2fa669f Mon Sep 17 00:00:00 2001 From: emanuel Date: Fri, 8 Apr 2022 15:44:37 +0000 Subject: [PATCH 2/7] Change PCM rectangle to circle intersection --- src/pcm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pcm.c b/src/pcm.c index a76821d..718c0c8 100644 --- a/src/pcm.c +++ b/src/pcm.c @@ -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; From fba82a5ca31f13ee9f40ed9f057b1b05af047ef8 Mon Sep 17 00:00:00 2001 From: emanuel Date: Fri, 8 Apr 2022 17:31:20 +0000 Subject: [PATCH 3/7] Lost PCM.replies --- src/pcm.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/pcm.c b/src/pcm.c index 718c0c8..a38fca4 100644 --- a/src/pcm.c +++ b/src/pcm.c @@ -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 is response to another ITS-S PCM.request"); + } else { + syslog_info("[facilities] [pc] received PCM.reply with %d accepted trajectories | took %ld us", reply->acceptedTrajectoriesIds.list.count, now_us-neighbour->t_iid); + } + neighbour->intersecting = false; neighbour->proposed = false; From 2a205e568eb0ad117aadceb183ceff02e9ccc71f Mon Sep 17 00:00:00 2001 From: Emanuel Vieira Date: Fri, 8 Apr 2022 20:51:34 +0000 Subject: [PATCH 4/7] Fix PCM handle reply info log --- src/pcm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pcm.c b/src/pcm.c index a38fca4..ca80e32 100644 --- a/src/pcm.c +++ b/src/pcm.c @@ -220,9 +220,9 @@ static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm, mc_neigh pthread_mutex_unlock(&facilities->epv.time.lock); if (neighbour->intersecting) { - syslog_info("[facilities] [pc] received PCM.reply is response to another ITS-S PCM.request"); - } else { 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"); } From 3443e2e355549a380110aad2a3a984a22f52d2c2 Mon Sep 17 00:00:00 2001 From: emanuel Date: Mon, 11 Apr 2022 10:06:19 +0100 Subject: [PATCH 5/7] Reply to TPM, PCM if these are active --- src/facilities.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/facilities.c b/src/facilities.c index 069ea06..c691852 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -225,12 +225,16 @@ 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); - fwd = true; + if (facilities->coordination.active) { + pcm_check(facilities, its_msg); + fwd = true; + } break; default: From 827ac4f53dd8b4e82d47e6d29fabe5baf62cb722 Mon Sep 17 00:00:00 2001 From: emanuel Date: Mon, 11 Apr 2022 15:01:40 +0100 Subject: [PATCH 6/7] Forward PCM to applications even if PCM is inactive --- src/facilities.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/facilities.c b/src/facilities.c index c691852..9f26fad 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -233,8 +233,8 @@ static int transport_indication(facilities_t *facilities, void* responder, void* case 2043: if (facilities->coordination.active) { pcm_check(facilities, its_msg); - fwd = true; } + fwd = true; break; default: From 85f0cfe04b3928bc285d09916fcdacc9a6e15568 Mon Sep 17 00:00:00 2001 From: emanuel Date: Wed, 13 Apr 2022 11:48:06 +0100 Subject: [PATCH 7/7] Fix SAEM Tolling TLS TPI encoding --- src/saem.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/saem.c b/src/saem.c index ee52fae..c5b5fa5 100644 --- a/src/saem.c +++ b/src/saem.c @@ -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;