diff --git a/src/config.c b/src/config.c index c761f8c..efa0174 100644 --- a/src/config.c +++ b/src/config.c @@ -207,6 +207,8 @@ int facilities_config(void* facilities_s) { facilities->dissemination->active = config->facilities.cpm.activate; facilities->dissemination->T_GenCpmMin = config->facilities.cpm.rsu_obu_period_min; facilities->dissemination->T_GenCpmMax = config->facilities.cpm.rsu_obu_period_max; + facilities->dissemination->radar_rotation = config->applications.its_center.rotation; + // Replay facilities->replay = config->networking.replay.activate; diff --git a/src/cpm.c b/src/cpm.c index e19c6a6..7a0fea1 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -10,7 +10,7 @@ #include #include #include - +#include #include #include @@ -449,6 +449,7 @@ void mk_cpm(facilities_t* facilities,CPM_t* cpm_tx, struct timespec* systemtime) 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) { @@ -523,38 +524,37 @@ void *cp_service(void *fc){ /* Variables */ int i32_recv_bytes; u_int8_t au8_readBuffer[READ_BUFFER_SIZE]; - uint8_t bdr_oer[2048]; - bdr_oer[0] = 4; //Facilities bool is_radar_connected; - + + uint8_t bdr_oer[2048]; + uint8_t fdi_oer[2048]; + bdr_oer[0] = 4; //Facilities + fdi_oer[0] = 4; + facilities_t *facilities = (facilities_t *) fc; BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); + FacilitiesDataIndication_t *fdi = calloc(1, sizeof(FacilitiesDataIndication_t)); - //roadRotationSin = sin(((config->applications.its_center.rotation + 90.0) * PI) / 180); - //roadRotationCos = cos(((config->applications.its_center.rotation + 90.0) * PI) / 180); + roadRotationSin = sin(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180); + roadRotationCos = cos(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180); - - roadRotationSin = 0.8660; // Default values for A3 Configuration - roadRotationCos = -0.5000; // Default values for A3 configuration - - - /* Fill mandatory BTP Data Request parameters */ + /*--- Fill mandatory BTP Data Request parameters ---*/ bdr->gnDestinationAddress.buf = malloc(6); bdr->gnDestinationAddress.size = 6; for(int i = 0; i < 6; i++) - bdr->gnDestinationAddress.buf[i] = 0xff; //Broadcast addr + bdr->gnDestinationAddress.buf[i] = 0xff; //Broadcast addr - bdr->btpType = BTPType_btpB; //BTP Type B is for non-interactive packet transport | BTP Type A is for interactive packet transport - //The former doesn't have a source port and the latter have + bdr->btpType = BTPType_btpB; //BTP Type B is for non-interactive packet transport | BTP Type A is for interactive packet transport + //The former doesn't have a source port and the latter have - bdr->destinationPort = Port_cpm; //CPM entity port for communication between Facilities and Transport - bdr->gnPacketTransportType = PacketTransportType_shb; //shb = Single Hop Broadcast packet - bdr->gnTrafficClass = 2; //Identifier assigned to a GeoNetworking packet that expresses its requirements on data transport (WHY IS IT 2?) - bdr->data.buf = malloc(1500); // CPM Data to be sent to the Transport layer + bdr->destinationPort = Port_cpm; //CPM entity port for communication between Facilities and Transport + bdr->gnPacketTransportType = PacketTransportType_shb; //shb = Single Hop Broadcast packet + bdr->gnTrafficClass = 2; //Identifier assigned to a GeoNetworking packet that expresses its requirements on data transport + bdr->data.buf = malloc(1500); //CPM Data to be sent to the Transport layer if(facilities->use_security) { bdr->gnSecurityProfile = malloc(sizeof(long)); @@ -562,7 +562,14 @@ void *cp_service(void *fc){ } is_radar_connected = interface_connection(RADAR_IP,RADAR_PORT); // 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) { @@ -590,31 +597,49 @@ void *cp_service(void *fc){ /* Information parsing */ parse_input(au8_readBuffer,i32_recv_bytes); - /* CPM build and encoding (Could be merged if needed) */ + /* 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); 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 = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047); - if(enc.encoded == -1){ + 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"); continue; } + /* 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"); + continue; + } + /* Create thread to send packet to the Transport Layer (=3) */ - queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3); + queue_add(facilities->tx_queue, bdr_oer, enc_bdr.encoded+1, 3); + pthread_cond_signal(&facilities->tx_queue->trigger); + + /* Create thread to send packet to the Applications Layer (=5) */ + queue_add(facilities->tx_queue, fdi_oer, enc_fdi.encoded+1, 5); pthread_cond_signal(&facilities->tx_queue->trigger); /*Reset Timer for dissemination control */ - + dissemination_reset_timer(facilities->dissemination); - /*TODO: Send message to applications to send it to the broker ? *FacilitiesDataIndication*/ }else{ /* Waiting for Radar to reconnect */ @@ -624,5 +649,6 @@ void *cp_service(void *fc){ } ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr); + ASN_STRUCT_FREE(asn_DEF_FacilitiesDataIndication,fdi); return NULL; } diff --git a/src/cpm.h b/src/cpm.h index 2e514e5..9a3e831 100644 --- a/src/cpm.h +++ b/src/cpm.h @@ -163,10 +163,12 @@ typedef struct uint64_t next_cpm_min; - uint64_t T_GenCpmMin; uint64_t T_GenCpmMax; + /* Position of the radar (Value from toml) */ + int8_t radar_rotation; + } dissemination_t; /* Prototype Functions */