Added FacilitiesDataIndication forward (CPM)

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;

src/cpm.c

@@ -10,7 +10,7 @@
 #include <math.h>
 #include <signal.h>
 #include <unistd.h>
-
+#include <itss-facilities/FacilitiesDataIndication.h>
 
 #include <syslog.h>
 #include <arpa/inet.h>
@@ -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);
+	roadRotationSin = sin(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180);
- 	//roadRotationCos = cos(((config->applications.its_center.rotation + 90.0) * PI) / 180);
+ 	roadRotationCos = cos(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180);
 
-  
+	/*--- Fill mandatory BTP Data Request parameters ---*/
-	roadRotationSin = 0.8660;           // Default values for A3 Configuration
-  	roadRotationCos = -0.5000;          // Default values for A3 configuration
-
-
-	/* 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
+	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
+					 //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->destinationPort = Port_cpm;			 //CPM entity port for communication between Facilities and Transport
-	bdr->gnPacketTransportType = PacketTransportType_shb;                //shb = Single Hop Broadcast packet
+	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->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
+    	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);
+				asn_enc_rval_t enc_bdr = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047);
-				if(enc.encoded == -1){
+				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; 
 }

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 */