diff --git a/src/config.c b/src/config.c index fe47b8d..3525216 100644 --- a/src/config.c +++ b/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; diff --git a/src/dcm.c b/src/dcm.c index 99b34ee..59fe5de 100644 --- a/src/dcm.c +++ b/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; diff --git a/src/dcm.h b/src/dcm.h index a2ad9b6..a226bb0 100644 --- a/src/dcm.h +++ b/src/dcm.h @@ -1,4 +1,12 @@ #pragma once +#include + +typedef struct coordination { + bool active; + +} coordination_s; + +int dcm_check(); void* dc_service(void* fc); diff --git a/src/facilities.c b/src/facilities.c index 04f923e..cc0802c 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -6,6 +6,7 @@ #include "requests.h" #include "cpm.h" #include "saem.h" +#include "dcm.h" #include #include @@ -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)); diff --git a/src/facilities.h b/src/facilities.h index 38b2b03..5a24e68 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -12,6 +12,7 @@ #include "cpm.h" #include "saem.h" #include "tpm.h" +#include "dcm.h" #include #include @@ -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;