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-06-08 17:10:49 +01:00
parent af5cb92560 91792d00e1
commit a52f8d3bb1
1 changed files with 57 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
src/cpm.c
View File

@ -66,7 +66,7 @@ bool waitingIncomingConnection(void){
bool interface_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){
@ -404,7 +404,10 @@ long rotate_y(long x, long y) {
} }
void mk_cpm(facilities_t* facilities,CPM_t* cpm_tx, struct timespec* systemtime) { 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;
CPM_t* cpm_tx = calloc(1, sizeof(CPM_t));
cpm_tx->header.protocolVersion = PROTOCOL_VERSION; cpm_tx->header.protocolVersion = PROTOCOL_VERSION;
@ -475,21 +478,35 @@ void mk_cpm(facilities_t* facilities,CPM_t* cpm_tx, struct timespec* systemtime)
} }
cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count;
}
/* Encode CPMs to FDI and BDR */
int encode_cpm(CPM_t* cpm_tx, uint8_t *out_message_data, uint32_t *out_message_size) { //BDR
memset(bdr_oer, 0, 1500);
memset(out_message_data, 0, 1500); asn_enc_rval_t retval_enc_bdr = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, bdr_oer, 1500);
asn_enc_rval_t retval_enc = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, out_message_data, 1500); if (retval_enc_bdr.encoded == -1) {
if (retval_enc.encoded == -1) { syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc_bdr.failed_type->name);
syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc.failed_type->name); rv = 1;
return 1; goto cleanup;
} }
*out_message_size = ((retval_enc.encoded + 7) / 8); *bdr_len = ((retval_enc_bdr.encoded + 7) / 8);
return 0; //FDI
memset(fdi_oer, 0, 1500);
asn_enc_rval_t retval_enc_fdi = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, fdi_oer, 1500);
if (retval_enc_fdi.encoded == -1) {
syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc_fdi.failed_type->name);
rv = 1;
goto cleanup;
}
*fdi_len = ((retval_enc_fdi.encoded + 7) / 8);
cleanup:
ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx);
return rv;
} }
dissemination_t* dissemination_init(){ dissemination_t* dissemination_init(){
@ -511,9 +528,11 @@ int dissemination_check(dissemination_t* dissemination, it2s_tender_epv_t* epv)
pthread_mutex_lock(&dissemination->lock); pthread_mutex_lock(&dissemination->lock);
// Both cases for OBU and RSU (BASIC: generation interval 1s) // Both cases for OBU and RSU (BASIC: generation interval 1s)
if (now >= dissemination->next_cpm_max) if (now >= dissemination->next_cpm_min){
rv = 1; rv = 1;
}else if(now >= dissemination->next_cpm_max){
rv = 0;
}
pthread_mutex_unlock(&dissemination->lock); pthread_mutex_unlock(&dissemination->lock);
@ -537,7 +556,6 @@ void dissemination_reset_timer(dissemination_t* dissemination, it2s_tender_epv_t
void *cp_service(void *fc){ void *cp_service(void *fc){
/* Variables */ /* Variables */
int i32_recv_bytes; int i32_recv_bytes;
@ -548,7 +566,7 @@ void *cp_service(void *fc){
uint8_t bdr_oer[2048]; uint8_t bdr_oer[2048];
uint8_t fdi_oer[2048]; uint8_t fdi_oer[2048];
bdr_oer[0] = 4; //Facilities bdr_oer[0] = 4; //Facilities
fdi_oer[0] = 4; fdi_oer[0] = 4;
BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t));
@ -558,7 +576,6 @@ void *cp_service(void *fc){
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 ---*/ /*--- Fill mandatory BTP Data Request parameters ---*/
bdr->gnDestinationAddress.buf = malloc(6); bdr->gnDestinationAddress.buf = malloc(6);
bdr->gnDestinationAddress.size = 6; bdr->gnDestinationAddress.size = 6;
for(int i = 0; i < 6; i++) for(int i = 0; i < 6; i++)
@ -579,39 +596,28 @@ void *cp_service(void *fc){
*bdr->gnSecurityProfile = 1; *bdr->gnSecurityProfile = 1;
} }
is_radar_connected = interface_connection(RADAR_PORT,facilities); // Create Radar listening socket
/*--- Fill mandatory Facilities Data Indication parameters ---*/ /*--- Fill mandatory Facilities Data Indication parameters ---*/
fdi->itsMessageType = ItsMessageType_cpm; fdi->itsMessageType = ItsMessageType_cpm;
fdi->data.buf = malloc(1500); fdi->data.buf = malloc(1500);
struct timespec* systemtime; /* Creating sockets and waiting for radar to connect*/
systemtime = (struct timespec*)malloc(sizeof(struct timespec)); is_radar_connected = radar_connection(RADAR_PORT,facilities);
if (systemtime == NULL) {
printf("memory allocation failed: %m");
exit(1);
}
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 */
/* 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) if(facilities->dissemination->tmc_connect == false)
i32_recv_bytes = recv(s_socket.i32_client, &au8_readTcp, READ_BUFFER_SIZE, 0); i32_recv_bytes = recv(s_socket.i32_client, &au8_readTcp, READ_BUFFER_SIZE, 0);
/* Reads from the radar */ /* Reads from the radar */
CPM_t* cpm_tx = calloc(1, sizeof(CPM_t)); i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0);
i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0); //recv(socket,buffer,size,flags)
if (dissemination_check(facilities->dissemination, &facilities->epv) && facilities->dissemination->active){ if (dissemination_check(facilities->dissemination, &facilities->epv) && facilities->dissemination->active){
if(is_radar_connected){ if(is_radar_connected){
if(i32_recv_bytes <= 0){ if(i32_recv_bytes <= 0){
syslog_debug("No data received from radar ..."); syslog_debug("No data received from radar ...");
is_radar_connected = false; is_radar_connected = false;
@ -619,27 +625,14 @@ void *cp_service(void *fc){
} }
/* Information parsing */ /* Information parsing from radar */
parse_input(au8_readBuffer,i32_recv_bytes); parse_input(au8_readBuffer,i32_recv_bytes);
/* CPM build and encoding to bdr and fdi (Could be merged if needed) */ /* CPM build and encoding to BDR and FDI */
mk_cpm(facilities,cpm_tx,systemtime); if(mk_cpm(facilities, bdr->data.buf, (uint32_t *) &bdr->data.size, fdi->data.buf, (uint32_t *) &fdi->data.size) == 1)
if(encode_cpm(cpm_tx, bdr->data.buf, (uint32_t *) &bdr->data.size) == 1){
ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx);
continue; continue;
}
if(encode_cpm(cpm_tx, fdi->data.buf, (uint32_t *) &fdi->data.size) == 1){
ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx);
continue;
}
ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx);
/* Encode BTPDataRequest */ /* Encode BTPDataRequest */
asn_enc_rval_t enc_bdr = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047); asn_enc_rval_t enc_bdr = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047);
if(enc_bdr.encoded == -1){ if(enc_bdr.encoded == -1){
syslog_err("[facilities] encoding BTPDataRequest for cpm failed"); syslog_err("[facilities] encoding BTPDataRequest for cpm failed");
@ -647,7 +640,6 @@ void *cp_service(void *fc){
} }
/* Encode FacilitiesDataIndication */ /* Encode FacilitiesDataIndication */
asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesDataIndication, NULL, fdi, fdi_oer+1, 2047); asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesDataIndication, NULL, fdi, fdi_oer+1, 2047);
if(enc_fdi.encoded == -1){ if(enc_fdi.encoded == -1){
syslog_err("[facilities] encoding FacilitiesDataIndication for cpm failed"); syslog_err("[facilities] encoding FacilitiesDataIndication for cpm failed");
@ -675,5 +667,11 @@ void *cp_service(void *fc){
ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr); ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr);
ASN_STRUCT_FREE(asn_DEF_FacilitiesDataIndication,fdi); ASN_STRUCT_FREE(asn_DEF_FacilitiesDataIndication,fdi);
return NULL;
/* Close sockets */
if(facilities->dissemination->tmc_connect)
shutdown(s_socket.i32_socket,2);
shutdown(raw_socket.raw_fd,2);
return NULL;
} }