Config triggered SAEMs

src/config.c

@@ -367,6 +367,13 @@ int facilities_config(void* facilities_s) {
     ASN_STRUCT_FREE(asn_DEF_ManagementReply, mrep);
     zmq_close(management_socket);
 
+    if (config->facilities.saem.activate) {
+        facilities->bulletin.to_provide_len = 1;
+        facilities->bulletin.to_provide[0] = calloc(1, sizeof(announcement_t));
+        facilities->bulletin.to_provide[0]->endpoint.port = 7777;
+        facilities->bulletin.to_provide[0]->its_aid = config->facilities.saem.service_to_advertise;
+    }
+
     facilities->vehicle.length = config->facilities.vehicle.length;
     facilities->vehicle.width = config->facilities.vehicle.width;
     facilities->vehicle.role = config->facilities.vehicle.role;

src/facilities.c

@@ -512,6 +512,7 @@ int main() {
     facilities.tx_queue = queue_init();
     facilities.den = calloc(1, sizeof(den_t));
     facilities.infrastructure = calloc(1, sizeof(infrastructure_t));
+    bulletin_init(&facilities.bulletin);
 
     time_t t;
     srand((unsigned) time(&t));
@@ -537,8 +538,7 @@ int main() {
         pthread_create(&facilities.cp_service, NULL, cp_service, (void*) &facilities);
 
     // SA
-    //bulletin_init(&facilities.bulletin);
+    pthread_create(&facilities.sa_service, NULL, sa_service, (void*) &facilities);
-    //pthread_create(&facilities.sa_service, NULL, sa_service, (void*) &facilities);
 
     uint8_t buffer[PACKET_MAX_LEN];
     syslog_info("[facilities] listening");

src/sa.c

@@ -31,6 +31,7 @@ SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem) {
         return SAEM_INVALID_HEADER_VERSION;
     }
 
+    pthread_mutex_lock(&bulletin->lock);
 
     if (saem->sam.body.serviceInfos) {
         for (int i = 0; i < saem->sam.body.serviceInfos->list.count; ++i) {
@@ -38,11 +39,8 @@ SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem) {
 
             uint16_t its_aid = si->serviceID;
 
-            if (!its_aid) {
-                continue;
-            }
-
             int index = -1;
+
             for (int a = 0; a < bulletin->to_consume_len; ++a) {
                 /* Check existence through stationID and itsAid */
                 if (saem->header.stationID == bulletin->to_consume[a]->station_id && 
@@ -91,6 +89,8 @@ SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem) {
         }
     }
 
+    pthread_mutex_unlock(&bulletin->lock);
+
     return rv;
 }
 
@@ -102,6 +102,11 @@ void bulletin_init(bulletin_t* bulletin) {
     for (int i = 0; i < MAX_ANNOUNCEMENTS_LEN; ++i) {
         bulletin->to_consume[i] = calloc(1, sizeof(announcement_t));
     }
+
+    bulletin->to_provide_len = 1;
+    bulletin->to_provide[0] = calloc(1, sizeof(announcement_t));
+    bulletin->to_provide[0]->endpoint.port = 7777;
+
 }
 
 int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) {
@@ -110,7 +115,7 @@ int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) {
     SAEM_t* saem = calloc(1, sizeof(SAEM_t));
 
     /* header */
-    saem->header.protocolVersion = 2;
+    saem->header.protocolVersion = 1;
     saem->header.messageID = messageID_saem;
 
     pthread_mutex_lock(&facilities->id.lock);
@@ -255,7 +260,7 @@ void *sa_service(void *fc) {
         }
         pthread_mutex_unlock(&bulletin->lock);
 
-        sleep(1);
+        usleep(100000);
     }
 
     ASN_STRUCT_FREE(asn_DEF_TransportDataRequest, tdr);