Merge branch 'master' of https://gitlab.es.av.it.pt/its/it2s-itss-facilities
This commit is contained in:
emanuel
commit
a52f8d3bb1
94
src/cpm.c
94
src/cpm.c
|
|
@ -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){
|
||||
|
||||
|
|
@ -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;
|
||||
|
|
@ -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;
|
||||
|
||||
/* Encode CPMs to FDI and BDR */
|
||||
|
||||
//BDR
|
||||
memset(bdr_oer, 0, 1500);
|
||||
asn_enc_rval_t retval_enc_bdr = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, bdr_oer, 1500);
|
||||
if (retval_enc_bdr.encoded == -1) {
|
||||
syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc_bdr.failed_type->name);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
*bdr_len = ((retval_enc_bdr.encoded + 7) / 8);
|
||||
|
||||
int encode_cpm(CPM_t* cpm_tx, uint8_t *out_message_data, uint32_t *out_message_size) {
|
||||
|
||||
memset(out_message_data, 0, 1500);
|
||||
asn_enc_rval_t retval_enc = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, out_message_data, 1500);
|
||||
if (retval_enc.encoded == -1) {
|
||||
syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc.failed_type->name);
|
||||
return 1;
|
||||
//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;
|
||||
}
|
||||
|
||||
*out_message_size = ((retval_enc.encoded + 7) / 8);
|
||||
*fdi_len = ((retval_enc_fdi.encoded + 7) / 8);
|
||||
|
||||
return 0;
|
||||
cleanup:
|
||||
ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx);
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
dissemination_t* dissemination_init(){
|
||||
|
|
@ -511,9 +528,11 @@ int dissemination_check(dissemination_t* dissemination, it2s_tender_epv_t* epv)
|
|||
pthread_mutex_lock(&dissemination->lock);
|
||||
|
||||
// Both cases for OBU and RSU (BASIC: generation interval 1s)
|
||||
if (now >= dissemination->next_cpm_max)
|
||||
if (now >= dissemination->next_cpm_min){
|
||||
rv = 1;
|
||||
|
||||
}else if(now >= dissemination->next_cpm_max){
|
||||
rv = 0;
|
||||
}
|
||||
|
||||
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){
|
||||
|
||||
/* Variables */
|
||||
|
||||
int i32_recv_bytes;
|
||||
|
|
@ -558,7 +576,6 @@ void *cp_service(void *fc){
|
|||
roadRotationCos = cos(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180);
|
||||
|
||||
/*--- Fill mandatory BTP Data Request parameters ---*/
|
||||
|
||||
bdr->gnDestinationAddress.buf = malloc(6);
|
||||
bdr->gnDestinationAddress.size = 6;
|
||||
for(int i = 0; i < 6; i++)
|
||||
|
|
@ -579,39 +596,28 @@ void *cp_service(void *fc){
|
|||
*bdr->gnSecurityProfile = 1;
|
||||
}
|
||||
|
||||
is_radar_connected = interface_connection(RADAR_PORT,facilities); // Create Radar listening socket
|
||||
|
||||
|
||||
/*--- Fill mandatory Facilities Data Indication parameters ---*/
|
||||
|
||||
fdi->itsMessageType = ItsMessageType_cpm;
|
||||
fdi->data.buf = malloc(1500);
|
||||
|
||||
struct timespec* systemtime;
|
||||
systemtime = (struct timespec*)malloc(sizeof(struct timespec));
|
||||
if (systemtime == NULL) {
|
||||
printf("memory allocation failed: %m");
|
||||
exit(1);
|
||||
}
|
||||
/* Creating sockets and waiting for radar to connect*/
|
||||
is_radar_connected = radar_connection(RADAR_PORT,facilities);
|
||||
|
||||
|
||||
while(!facilities->exit){
|
||||
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)
|
||||
i32_recv_bytes = recv(s_socket.i32_client, &au8_readTcp, READ_BUFFER_SIZE, 0);
|
||||
|
||||
/* 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); //recv(socket,buffer,size,flags)
|
||||
|
||||
i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0);
|
||||
|
||||
if (dissemination_check(facilities->dissemination, &facilities->epv) && facilities->dissemination->active){
|
||||
if(is_radar_connected){
|
||||
|
||||
|
||||
if(i32_recv_bytes <= 0){
|
||||
syslog_debug("No data received from radar ...");
|
||||
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);
|
||||
|
||||
/* CPM build and encoding to bdr and fdi (Could be merged if needed) */
|
||||
mk_cpm(facilities,cpm_tx,systemtime);
|
||||
|
||||
|
||||
if(encode_cpm(cpm_tx, bdr->data.buf, (uint32_t *) &bdr->data.size) == 1){
|
||||
ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx);
|
||||
/* CPM build and encoding to BDR and FDI */
|
||||
if(mk_cpm(facilities, bdr->data.buf, (uint32_t *) &bdr->data.size, fdi->data.buf, (uint32_t *) &fdi->data.size) == 1)
|
||||
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 */
|
||||
|
||||
asn_enc_rval_t enc_bdr = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047);
|
||||
if(enc_bdr.encoded == -1){
|
||||
syslog_err("[facilities] encoding BTPDataRequest for cpm failed");
|
||||
|
|
@ -647,7 +640,6 @@ void *cp_service(void *fc){
|
|||
}
|
||||
|
||||
/* Encode FacilitiesDataIndication */
|
||||
|
||||
asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesDataIndication, NULL, fdi, fdi_oer+1, 2047);
|
||||
if(enc_fdi.encoded == -1){
|
||||
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_FacilitiesDataIndication,fdi);
|
||||
|
||||
/* Close sockets */
|
||||
if(facilities->dissemination->tmc_connect)
|
||||
shutdown(s_socket.i32_socket,2);
|
||||
|
||||
shutdown(raw_socket.raw_fd,2);
|
||||
return NULL;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue