DCM efforts

src/config.c

@@ -314,6 +314,8 @@ int facilities_config(void* facilities_s) {
     }
     facilities->tolling.client_id = config->facilities.tpm.client_id;
 
+    // DCM
+    facilities->coordination.active = config->facilities.dcm.activate;
 
     // Replay
     facilities->replay = config->networking.replay.activate;

src/dcm.c

@@ -7,9 +7,16 @@
 
 #define SLEEP_TIME_MS 100
 
+int dcm_check() {
+
+    return 0;
+}
+
 int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len) {
     int rv = 0;
 
+    it2s_tender_st_s* trajectory = NULL;
+
     DCM_t* dcm = calloc(1, sizeof(DCM_t));
 
     dcm->header.messageID = 43;
@@ -20,14 +27,30 @@ int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len)
     pthread_mutex_unlock(&facilities->id.lock);
 
     int32_t lat, lon;
+    uint16_t trajectory_len = 0;
     it2s_tender_lock_space(&facilities->epv);
     it2s_tender_get_space(&facilities->epv);
-    dcm->dcm.currentPosition.latitude = lat;
-    dcm->dcm.currentPosition.longitude = lon;
+    lat = facilities->epv.space.latitude;
+    lon = facilities->epv.space.longitude;
+    trajectory_len = facilities->epv.trajectory.len;
+    trajectory = malloc(trajectory_len * sizeof(it2s_tender_st_s));
+    memcpy(trajectory, facilities->epv.trajectory.path, trajectory_len * sizeof(it2s_tender_st_s));
     it2s_tender_unlock_space(&facilities->epv);
 
+    dcm->dcm.currentPosition.latitude = lat;
+    dcm->dcm.currentPosition.longitude = lon;
     asn_ulong2INTEGER(&dcm->dcm.currentPosition.timestamp, now);
 
+    dcm->dcm.plannedTrajectory.list.count = trajectory_len;
+    dcm->dcm.plannedTrajectory.list.size = trajectory_len * sizeof(void*);
+    dcm->dcm.plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*));
+    for (int i = 0; i < trajectory_len; ++i) {
+        dcm->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
+        dcm->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i].latitude;
+        dcm->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i].longitude;
+        asn_ulong2INTEGER(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i].timestamp);
+    }
+
     asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_DCM, NULL, dcm, dcm_uper, 512);
     if (enc.encoded == -1) {
         syslog_err("[facilities] [dc] failed encoding DCM (%s)", enc.failed_type->name);
@@ -37,6 +60,9 @@ int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len)
     *dcm_uper_len = (enc.encoded + 7) / 8;
 
 cleanup:
+    ASN_STRUCT_FREE(asn_DEF_DCM, dcm);
+    free(trajectory);
+
     return rv;
 }
 
@@ -68,6 +94,7 @@ void* dc_service(void* fc) {
     bpr->data.buf = malloc(512);
 
     uint8_t tr_oer[1024];
+    tr_oer[0] = 4;
 
     int rv;
 

src/dcm.h

@@ -1,4 +1,12 @@
 #pragma once
 
+#include <stdbool.h>
+
+typedef struct coordination {
+    bool active;
+
+} coordination_s;
+
+int dcm_check();
 
 void* dc_service(void* fc);

src/facilities.c

@@ -6,6 +6,7 @@
 #include "requests.h"
 #include "cpm.h"
 #include "saem.h"
+#include "dcm.h"
 
 #include <itss-networking/NetworkingIndication.h>
 #include <itss-transport/TransportRequest.h>
@@ -684,6 +685,10 @@ int main() {
     // Tolling
     tolling_init(&facilities.tolling, facilities.zmq.ctx, facilities.zmq.security_address);
 
+    // DC
+    if (facilities.coordination.active)
+        pthread_create(&facilities.dc_service, NULL, dc_service, (void*) &facilities);
+
     void* security_socket = zmq_socket(facilities.zmq.ctx, ZMQ_REQ);
     int wait_ms = 1000;
     zmq_setsockopt(security_socket, ZMQ_RCVTIMEO, &wait_ms, sizeof(int));

src/facilities.h

@@ -12,6 +12,7 @@
 #include "cpm.h"
 #include "saem.h"
 #include "tpm.h"
+#include "dcm.h"
 
 #include <it2s-tender/epv.h>
 #include <it2s-tender/syslog.h>
@@ -30,6 +31,7 @@ typedef struct facilities {
     pthread_t transmitting;
     pthread_t cp_service;
     pthread_t sa_service;
+    pthread_t dc_service;
 
     // ZMQ
     struct {
@@ -64,6 +66,9 @@ typedef struct facilities {
     // TP
     tolling_s tolling;
 
+    // DC
+    coordination_s coordination;
+
     int station_type;
     bool use_security;
     bool replay;