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

remove perceivedObjectContainer from the CPM when no objects are detected

This commit is contained in:
Mohannad Jooriah 2022-04-08 16:36:07 +01:00
parent 12e98954c1
commit 3b58068f78
1 changed files with 34 additions and 32 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;