working mcms upward

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

src/config.c

@@ -3,6 +3,7 @@
 #include "saem.h"
 #include "tpm.h"
 #include "vcm.h"
+#include "mcm.h"
 
 #include <string.h>
 #include <stdlib.h>
@@ -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("mcm", etsi_its_cfg->facilities.mcm.protocol)){
-    if (!strcmp("vcm-RR", etsi_its_cfg->facilities.mcm.protocol)) {
+        facilities.mcm_coord.protocol = MC_PROTOCOL_MCM; 
-        facilities.coordination.protocol = MC_PROTOCOL_VCM_RR;
+        facilities.mcm_coord.active = etsi_its_cfg->facilities.mcm.activate;
-    } else if (!strcmp("vcm-RR1C", etsi_its_cfg->facilities.mcm.protocol)) {
+        facilities.coordination.active = false;
-        facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C;
+    }
-    } else if (!strcmp("vcm-RRAC", etsi_its_cfg->facilities.mcm.protocol)) {
+    else if (!strcmp("vcm-RR", etsi_its_cfg->facilities.mcm.protocol)){
-        facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC;
+        facilities.coordination.protocol = MC_PROTOCOL_VCM_RR;
-    } else {
+        facilities.coordination.active = etsi_its_cfg->facilities.mcm.activate;
-        facilities.coordination.protocol = MC_PROTOCOL_VCM_RR;
+        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;

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);
 

src/facilities.h

@@ -14,6 +14,7 @@
 #include "vcm.h"
 #include "evm.h"
 #include "edm.h"
+#include "mcm.h"
 
 #include <it2s-tender/epv.h>
 #include <it2s-tender/database.h>
@@ -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;

src/mcm.c

@@ -0,0 +1,205 @@
+#include "mcm.h"
+#include "facilities.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <stdint.h>
+
+#include <it2s-gnss.h>
+#include <it2s-tender/epv.h>
+#include <it2s-tender/space.h>
+#include <it2s-tender/recorder.h>
+#include <it2s-tender/packet.h>
+#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
+#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
+#include <it2s-asn/etsi-its-v2/mcm/EI2_MCM.h>
+
+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);
+}

src/mcm.h

@@ -0,0 +1,23 @@
+#ifndef FACILITIES_MCM_H
+#define FACILITIES_MCM_H
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <pthread.h>
+#include <stdbool.h>
+
+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