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-01-04 16:58:44 +00:00
parent 79de90bdc6 d495925f2e
commit aae6772b56
1 changed files with 23 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
src/cpm.c
View File

@ -39,8 +39,6 @@
float roadRotationSin;
float roadRotationCos;
long history_list[NOF_OBJECTS][5];
int valid_array[NOF_OBJECTS];
S_ETHERNET_CONNECTION_T s_socket;
S_INTERFACE_CONNECTION_T raw_socket;
@ -356,7 +354,7 @@ void parse_can_data_tm(u_int32_t u32_can_id, int i32_can_len, u_int8_t* au8_can_
void parse_input(u_int8_t* u8_input_buffer, int i32_len) {
enum state_t s_state = IDLE;
static enum state_t s_state = IDLE;
u_int8_t u8_input = 0;
int i32_i;
@ -481,7 +479,7 @@ 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){
static void set_values(int i, int j, uint64_t timestamp, CPM_t* cpm_tx, long history_list[NOF_OBJECTS][4], int valid_array[], uint64_t history_timestamp[]){
/* 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;
@ -507,11 +505,11 @@ void set_values(int i, int j, uint64_t timestamp, CPM_t* cpm_tx){
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
history_timestamp[(long)as_objects[i].u8_objectID] = 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, long history_list[NOF_OBJECTS][4], int valid_array[], uint64_t history_timestamp[]) {
/* Variables */
CPM_t* cpm_tx = calloc(1, sizeof(CPM_t));
@ -576,13 +574,13 @@ static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len,
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));
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[(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);
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)
@ -601,8 +599,8 @@ static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len,
// 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){
set_values(i,j,generationDeltaTime,cpm_tx);
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
@ -654,26 +652,32 @@ void *cp_service(void *fc){
u_int8_t au8_readBuffer[READ_BUFFER_SIZE];
u_int8_t au8_readTcp[READ_BUFFER_SIZE];
bool is_radar_connected;
static long history_list[NOF_OBJECTS][4];
static int valid_array[NOF_OBJECTS];
static uint64_t history_timestamp[NOF_OBJECTS];
facilities_t *facilities = (facilities_t *) fc;
memset(history_list, 0, sizeof(history_list));
memset(valid_array, 0, sizeof(valid_array));
memset(history_timestamp, 0, sizeof(history_timestamp));
uint8_t tr_oer[2048];
uint8_t fi_oer[2048];
tr_oer[0] = 4; //Facilities
fi_oer[0] = 4;
TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t));
tr->present = TransportRequest_PR_packet;
TransportPacketRequest_t* tpr = &tr->choice.packet;
tpr->present = TransportPacketRequest_PR_btp;
TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t));
tr->present = TransportRequest_PR_packet;
TransportPacketRequest_t* tpr = &tr->choice.packet;
tpr->present = TransportPacketRequest_PR_btp;
BTPPacketRequest_t *bpr = &tpr->choice.btp;
FacilitiesIndication_t* fi = calloc(1, sizeof(FacilitiesIndication_t));
fi->present = FacilitiesIndication_PR_message;
FacilitiesMessageIndication_t* fmi = &fi->choice.message;
fi->present = FacilitiesIndication_PR_message;
FacilitiesMessageIndication_t* fmi = &fi->choice.message;
roadRotationSin = sin(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180);
roadRotationCos = cos(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180);
roadRotationCos = cos(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180);
/*--- Fill mandatory BTP Data Request parameters ---*/
bpr->gn.destinationAddress.buf = malloc(6);
@ -717,7 +721,7 @@ void *cp_service(void *fc){
parse_input(au8_readBuffer,i32_recv_bytes);
/* CPM build and encoding to BDR and FDI */
if(mk_cpm(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size) == 1)
if(mk_cpm(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size, history_list, valid_array, history_timestamp) == 1)
continue;
/* Encode TransportRequest */