diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c8294d8..3fa9939 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -12,6 +12,7 @@ ADD_EXECUTABLE(it2s-itss-facilities vcm.c evm.c edm.c + mcm.c ) TARGET_LINK_LIBRARIES(it2s-itss-facilities @@ -37,6 +38,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities -lit2s-asn-etsi-its-v2-cdd-2.2.1 -lit2s-asn-etsi-its-v2-cam -lit2s-asn-etsi-its-v2-denm + -lit2s-asn-etsi-its-v2-mcm -lit2s-tender -lm -lrt diff --git a/src/config.c b/src/config.c index 6587b9d..2e8d625 100644 --- a/src/config.c +++ b/src/config.c @@ -3,6 +3,7 @@ #include "saem.h" #include "tpm.h" #include "vcm.h" +#include "mcm.h" #include #include @@ -378,18 +379,31 @@ int facilities_config() { facilities.tolling.station.obu.client_id = etsi_its_cfg->facilities.tpm.client_id; // MCM - facilities.coordination.active = etsi_its_cfg->facilities.mcm.activate; - if (!strcmp("vcm-RR", etsi_its_cfg->facilities.mcm.protocol)) { - facilities.coordination.protocol = MC_PROTOCOL_VCM_RR; - } else if (!strcmp("vcm-RR1C", etsi_its_cfg->facilities.mcm.protocol)) { - facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C; - } else if (!strcmp("vcm-RRAC", etsi_its_cfg->facilities.mcm.protocol)) { - facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC; - } else { - facilities.coordination.protocol = MC_PROTOCOL_VCM_RR; + if (!strcmp("mcm", etsi_its_cfg->facilities.mcm.protocol)){ + facilities.mcm_coord.protocol = MC_PROTOCOL_MCM; + facilities.mcm_coord.active = etsi_its_cfg->facilities.mcm.activate; + facilities.coordination.active = false; + } + else if (!strcmp("vcm-RR", etsi_its_cfg->facilities.mcm.protocol)){ + facilities.coordination.protocol = MC_PROTOCOL_VCM_RR; + facilities.coordination.active = etsi_its_cfg->facilities.mcm.activate; + facilities.mcm_coord.active = false; + } + else if (!strcmp("vcm-RR1C", etsi_its_cfg->facilities.mcm.protocol)){ + facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C; + facilities.coordination.active = etsi_its_cfg->facilities.mcm.activate; + facilities.mcm_coord.active = false; + } + else if (!strcmp("vcm-RRAC", etsi_its_cfg->facilities.mcm.protocol)){ + facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC; + facilities.coordination.active = etsi_its_cfg->facilities.mcm.activate; + facilities.mcm_coord.active = false; + } + else { + log_error("[config] unrecognized mcm protocol"); + facilities.coordination.active = false; + facilities.mcm_coord.active = false; } - facilities.coordination.vcm_period_min = etsi_its_cfg->facilities.mcm.period_min; - facilities.coordination.vcm_period_max = etsi_its_cfg->facilities.mcm.period_max; // EVCSNM facilities.evm_args.activate = etsi_its_cfg->facilities.evcsnm.activate; diff --git a/src/facilities.c b/src/facilities.c index a792309..90a4d9c 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -414,6 +414,12 @@ int main() { edm_init(); } + // MCM + if (facilities.mcm_coord.active){ + mcm_coord_init(); + pthread_create(&facilities.mcm_service, NULL, mc_service, NULL); + } + facilities.apps_socket = itss_0connect(facilities.zmq.applications_address, ZMQ_REQ); security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); diff --git a/src/facilities.h b/src/facilities.h index ef04b79..b31e205 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -14,6 +14,7 @@ #include "vcm.h" #include "evm.h" #include "edm.h" +#include "mcm.h" #include #include @@ -43,6 +44,7 @@ typedef struct facilities { pthread_t sa_service; pthread_t vc_service; pthread_t evcsn_service; + pthread_t mcm_service; // ZMQ struct { @@ -82,7 +84,7 @@ typedef struct facilities { // TP tolling_t tolling; - // PC + // VC coordination_t coordination; // EVCSN @@ -91,6 +93,9 @@ typedef struct facilities { // EDM edm_t edm; + // MCM + mcm_coord_t mcm_coord; + // Logging struct { bool recorder; diff --git a/src/mcm.c b/src/mcm.c new file mode 100644 index 0000000..b754175 --- /dev/null +++ b/src/mcm.c @@ -0,0 +1,205 @@ +#include "mcm.h" +#include "facilities.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +static void tx_mcm(EI2_MCM_t* mcm){ + const uint16_t buf_len = 4096; + uint8_t buf[buf_len]; + EIS_NetworkingRequest_t *nr = NULL; + EIS_FacilitiesIndication_t *fi = NULL; + + asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EI2_MCM, NULL, mcm, buf, buf_len); + if (enc.encoded == -1){ + log_error("[mc] MCM encode failure (%s)", enc.failed_type->name); + goto cleanup; + } + + uint16_t mcm_len = (enc.encoded + 7) / 8; + + nr = calloc(1, sizeof(EIS_NetworkingRequest_t)); + nr->present = EIS_NetworkingRequest_PR_packet; + EIS_NetworkingPacketRequest_t *npr = &nr->choice.packet; + + npr->network.present = EIS_NetworkingPacketRequestNW_PR_gn; + npr->network.choice.gn.trafficClass = 2; + npr->network.choice.gn.destinationAddress.buf = malloc(6); + for (int i = 0; i < 6; ++i){ + npr->network.choice.gn.destinationAddress.buf[i] = 0xff; + } + npr->network.choice.gn.destinationAddress.size = 6; + npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb; + npr->network.choice.gn.securityProfile.sign = true; + + npr->transport.present = EIS_NetworkingPacketRequestTP_PR_btp; + npr->transport.choice.btp.btpType = EIS_BTPType_btpB; + npr->transport.choice.btp.destinationPort = EIS_Port_mcm; + + if (facilities.edm.enabled && mcm->payload.mcmContainer.present == EI2_McmContainer_PR_vehiclemaneuverContainer){ + edm_encap(buf, &mcm_len, buf_len, EIS_Port_mcm); + npr->transport.choice.btp.destinationPortInfo = calloc(1, sizeof(OCTET_STRING_t)); + npr->transport.choice.btp.destinationPortInfo->size = 2; + npr->transport.choice.btp.destinationPortInfo->buf = malloc(2); + *(uint16_t *)npr->transport.choice.btp.destinationPortInfo->buf = 0xed; + } + + npr->id = itss_id(buf, mcm_len); + npr->data.buf = malloc(mcm_len); + memcpy(npr->data.buf, buf, mcm_len); + npr->data.size = mcm_len; + buf[0] = ITSS_FACILITIES; + enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_EIS_NetworkingRequest, nr, buf + 1, buf_len - 1); + if (enc.encoded == -1){ + log_error("[mc] NR MCM.reply encode failure (%s)", enc.failed_type->name); + goto cleanup; + } + + itss_queue_send(facilities.tx_queue, itss_queue_packet_new(buf, enc.encoded + 1, ITSS_NETWORKING, npr->id, "NR.packet.btp")); + + fi = calloc(1, sizeof(EIS_FacilitiesIndication_t)); + fi->present = EIS_FacilitiesIndication_PR_message; + fi->choice.message.id = npr->id; + fi->choice.message.itsMessageType = EIS_Port_mcm; + fi->choice.message.data.size = npr->data.size; + fi->choice.message.data.buf = malloc(npr->data.size); + memcpy(fi->choice.message.data.buf, npr->data.buf, npr->data.size); + buf[0] = ITSS_FACILITIES; + enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_EIS_FacilitiesIndication, fi, buf + 1, buf_len - 1); + if (enc.encoded == -1){ + log_error("[mc] TR MCM.reply encode failure (%s)", enc.failed_type->name); + goto cleanup; + } + + itss_queue_send(facilities.tx_queue, itss_queue_packet_new(buf, enc.encoded + 1, ITSS_APPLICATIONS, npr->id, "FI.message")); + +cleanup: + ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingRequest, nr); + ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesIndication, fi); +} + +static int mk_mcm(){ + int rv = 0; + uint64_t gdt = itss_time_get() % 65536; + + // MCM + EI2_MCM_t* mcm = calloc(1, sizeof(EI2_MCM_t)); + + // Header + EI2_ItsPduHeader_t* header = &mcm->header; + header->protocolVersion = 2; + header->messageId = EI2_MessageId_mcm; + header->stationId = 0; + + // Payload + EI2_WrappedMcmInformationBlocks_t* payload = &mcm->payload; + + EI2_McmBasicContainer_t* basicContainer = &payload->basicContainer; + asn_uint642INTEGER(&basicContainer->generationDeltaTime, gdt); + basicContainer->stationType = 0; + basicContainer->deltaReferencePosition.deltaLatitude = 0; + basicContainer->deltaReferencePosition.deltaLongitude = 0; + basicContainer->deltaReferencePosition.deltaAltitude = 0; + + EI2_McmContainer_t* mcmContainer = &payload->mcmContainer; + mcmContainer->present = EI2_McmContainer_PR_vehiclemaneuverContainer; + EI2_VehiclemaneuverContainer_t* vmcContainer = &mcmContainer->choice.vehiclemaneuverContainer; + vmcContainer->mcmType = EI2_McmType_intent; + vmcContainer->maneuverId = 0; + vmcContainer->maneuverCoordinationConcept = EI2_ManeuverCoordinationConcept_agreementSeeking; + vmcContainer->maneuverCoordinationRational.present = EI2_ManeuverCoordinationRational_PR_maneuverCooperationGoal; + vmcContainer->maneuverCoordinationRational.choice.maneuverCooperationGoal = EI2_ManeuverCooperationGoal_vehicleInterception; + vmcContainer->maneuverExecutionStatus = EI2_ManeuverExecutionStatus_started; + vmcContainer->trajectoryId = 0; + vmcContainer->vehicleTrajectory.present = EI2_vehicleTrajectory_PR_wgs84Trajectory; + + EI2_VehicleCurrentState_t* vcs = &vmcContainer->vehicleCurrentState; + EI2_McmGenericCurrentState_t* mgcs = &vcs->mcmGenericCurrentState; + mgcs->mcmType = EI2_McmType_intent; + mgcs->maneuverId = 0; + mgcs->maneuverCoordinationConcept = EI2_ManeuverCoordinationConcept_agreementSeeking; + mgcs->maneuverCoordinationRational.present = EI2_ManeuverCoordinationRational_PR_maneuverCooperationGoal; + mgcs->maneuverCoordinationRational.choice.maneuverCooperationGoal = EI2_ManeuverCooperationGoal_vehicleInterception; + mgcs->maneuverExecutionStatus = EI2_ManeuverExecutionStatus_started; + EI2_VehicleAutomationState_t* vas = &vcs->vehicleAutomationState; + vas->humanDrivingLongitudinalAutomated = false; + vas->humanDrivenLateralAutomated = false; + vas->automatedDriving = false; + vcs->speed.speedValue = 0; + vcs->speed.speedConfidence = 1; + vcs->heading.headingValue = 0; + vcs->heading.headingConfidence = 1; + vcs->longitudinalAcceleration.longitudinalAccelerationValue = 0; + vcs->longitudinalAcceleration.longitudinalAccelerationConfidence = 0; + vcs->vehicleSize.vehicleLenth = 0; + vcs->vehicleSize.vehicleWidth = 0; + + EI2_Wgs84Trajectory_t* trajectory = &vmcContainer->vehicleTrajectory.choice.wgs84Trajectory; + trajectory->trajectoryPoints.list.count = 2; + trajectory->trajectoryPoints.list.size = sizeof(EI2_Wgs84TrajectoryPoint_t*); + trajectory->trajectoryPoints.list.array = calloc(trajectory->trajectoryPoints.list.count, trajectory->trajectoryPoints.list.size); + + for (int i = 0; i < trajectory->trajectoryPoints.list.count; i++){ + trajectory->trajectoryPoints.list.array[i] = calloc(1, sizeof(EI2_Wgs84TrajectoryPoint_t)); + EI2_Wgs84TrajectoryPoint_t* trajectory_point = trajectory->trajectoryPoints.list.array[i]; + trajectory_point->intermediatePoint.present = EI2_IntermediatePoint_PR_reference; + trajectory_point->longitudePosition = 0; + trajectory_point->latitudePosition = 0; + trajectory_point->speed.speedValue = 0; + trajectory_point->speed.speedConfidence = 1; + EI2_IntermediatePointReference_t* ipr = &trajectory_point->intermediatePoint.choice.reference; + ipr->referenceStartingPosition.latitude = 0; + ipr->referenceStartingPosition.longitude = 0; + ipr->referenceStartingPosition.positionConfidenceEllipse.semiMajorAxisLength = 0; + ipr->referenceStartingPosition.positionConfidenceEllipse.semiMinorAxisLength = 0; + ipr->referenceStartingPosition.positionConfidenceEllipse.semiMajorAxisOrientation = 0; + ipr->referenceStartingPosition.altitude.altitudeValue = 0; + ipr->referenceStartingPosition.altitude.altitudeConfidence = 1; + ipr->referenceHeading.headingValue = 0; + ipr->referenceHeading.headingConfidence = 1; + ipr->lane.lanePosition = 0; + ipr->lane.laneCount = 1; + ipr->timeOfPos = 0; + } + + tx_mcm(mcm); + +cleanup: + ASN_STRUCT_FREE(asn_DEF_EI2_MCM, mcm); + + return rv; +} + +void *mc_service(){ + int rv; + mcm_coord_t *mcm_coord = (mcm_coord_t *)&facilities.mcm_coord; + + while (!facilities.exit){ + sleep(1); + + pthread_mutex_lock(&mcm_coord->lock); + rv = mk_mcm(); + if (rv) + continue; + pthread_mutex_unlock(&mcm_coord->lock); + } + + return NULL; +} + +void mcm_coord_init(){ + mcm_coord_t *coord = &facilities.mcm_coord; + pthread_mutex_init(&coord->lock, NULL); +} diff --git a/src/mcm.h b/src/mcm.h new file mode 100644 index 0000000..390f39d --- /dev/null +++ b/src/mcm.h @@ -0,0 +1,23 @@ +#ifndef FACILITIES_MCM_H +#define FACILITIES_MCM_H + +#include +#include +#include +#include + +typedef enum MCM_PROTOCOL { + MC_PROTOCOL_MCM, + MC_PROTOCOL_MCM_RR +} MCM_PROTOCOL_e; + +typedef struct mcm_coord { + bool active; + MCM_PROTOCOL_e protocol; + pthread_mutex_t lock; +} mcm_coord_t; + +void* mc_service(); +void mcm_coord_init(); + +#endif