VCM: Speed change on VCM req rx

src/facilities.c

@@ -814,7 +814,6 @@ int main() {
     infrastructure_init();
     dissemination_init();
     bulletin_init();
-    coordination_init();
     void* security_socket = NULL;
 
     time_t t;
@@ -848,10 +847,11 @@ int main() {
     // Tolling
     tolling_init(facilities.station_type);
 
-    // PC
+    // VC
-    if (facilities.coordination.active)
+    if (facilities.coordination.active) {
+        coordination_init();
         pthread_create(&facilities.vc_service, NULL, vc_service, NULL);
-
+    }
 
     security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ);
 

src/vcm.c

@@ -9,6 +9,7 @@
 #include <it2s-tender/packet.h>
 #include <itss-transport/TransportRequest.h>
 #include <itss-facilities/FacilitiesIndication.h>
+#include <itss-management/ManagementRequest.h>
 #include <vcm/VCM.h>
 
 static int are_vehicles_intersecting(
@@ -72,12 +73,35 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
         return 0;
     }
 
+    const ssize_t buf_len = 512;
+    uint8_t buf1[buf_len], buf2[buf_len];
+
+    // Break a little bit
+    ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t));
+    mreq->present = ManagementRequest_PR_attributes;
+    mreq->choice.attributes.present = ManagementRequestAttributes_PR_set;
+    mreq->choice.attributes.choice.set.speed = calloc(1, sizeof(ManagementSpeedSet_t));
+    ManagementSpeedSet_t* mgss = mreq->choice.attributes.choice.set.speed;
+    mgss->rate = 5; /* km/h/s */
+    mgss->temporary = true; /* go back to original speed after a while */
+    mgss->type.present = ManagementSpeedSetType_PR_diff; /* differential change set */
+    mgss->type.choice.diff = -10; /* % */
+    asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_ManagementRequest, mreq, buf1, buf_len);
+    if (enc.encoded == -1) {
+        log_error("[vc] failed to encode MReq.speedSet (%s)", enc.failed_type->name);
+    }
+    itss_0send(coordination->mgmt_socket, buf1, enc.encoded);
+    if (itss_0recv_rt(&coordination->mgmt_socket, buf2, buf_len, buf1, enc.encoded, 500) == -1) {
+        log_error("[vc]-> MReq.speedSet ->[management] <TIMEOUT>");
+    }
+    ASN_STRUCT_FREE(asn_DEF_ManagementRequest, mreq);
+
+
+    // Respond
     VCM_t* vcm_rep = NULL;
     TransportRequest_t* tr = NULL;
     FacilitiesIndication_t* fi = NULL;
 
-    const ssize_t buf_len = 512;
-    uint8_t buf[buf_len];
 
     itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
     ssize_t trajectoryA_len = 0;
@@ -143,7 +167,7 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
     mvc_rep->negotiation->choice.reply.requesterId = vcm->header.stationID;
     mvc_rep->negotiation->choice.reply.nonce = request->nonce;
 
-    asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm_rep, buf, buf_len);
+    enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm_rep, buf1, buf_len);
     if (enc.encoded == -1) {
         log_error("[vc] VCM.reply encode failure (%s)", enc.failed_type->name);
         rv = 1;
@@ -156,7 +180,7 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
     tr->choice.packet.present = TransportPacketRequest_PR_btp;
     BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;
     bpr->btpType = BTPType_btpB;
-    bpr->id = itss_id(buf, vcm_rep_len);
+    bpr->id = itss_id(buf1, vcm_rep_len);
     bpr->gn.destinationAddress.buf = malloc(6);
     for (int i = 0; i < 6; ++i) {
         bpr->gn.destinationAddress.buf[i] = 0xff;
@@ -166,17 +190,17 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
     bpr->destinationPort = 2043;
     bpr->gn.trafficClass = 2;
     bpr->data.buf = malloc(vcm_rep_len);
-    memcpy(bpr->data.buf, buf, vcm_rep_len);
+    memcpy(bpr->data.buf, buf1, vcm_rep_len);
     bpr->data.size = vcm_rep_len;
-    buf[0] = 4;
+    buf1[0] = 4;
-    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1);
+    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf1+1, buf_len-1);
     if (enc.encoded == -1) {
         log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
         rv = 1;
         goto cleanup;
     }
 
-    itss_queue_send(facilities.tx_queue, buf, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp");
+    itss_queue_send(facilities.tx_queue, buf1, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp");
 
     fi = calloc(1, sizeof(FacilitiesIndication_t));
     fi->present = FacilitiesIndication_PR_message;
@@ -185,15 +209,15 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
     fi->choice.message.data.size = bpr->data.size;
     fi->choice.message.data.buf = malloc(bpr->data.size);
     memcpy(fi->choice.message.data.buf, bpr->data.buf, bpr->data.size);
-    buf[0] = 4;
+    buf1[0] = 4;
-    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, buf_len-1);
+    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf1+1, buf_len-1);
     if (enc.encoded == -1) {
         log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
         rv = 1;
         goto cleanup;
     }
 
-    itss_queue_send(facilities.tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");
+    itss_queue_send(facilities.tx_queue, buf1, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");
 
     if (facilities.logging.recorder) {
         uint16_t buffer_len = 2048;
@@ -737,9 +761,13 @@ void* vc_service() {
         usleep(50 * 1000);
     }
 
+    itss_0close(coordination->mgmt_socket);
+
     return NULL;
 }
 
-void coordination_init(coordination_t* coordination) {
+void coordination_init() {
-    pthread_mutex_init(&coordination->lock, NULL);
+    coordination_t* coo = &facilities.coordination;
+    pthread_mutex_init(&coo->lock, NULL);
+    coo->mgmt_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ);
 }

src/vcm.h

@@ -27,6 +27,8 @@ typedef struct coordination {
     uint64_t vcm_period_min;
     uint64_t vcm_period_max;
 
+    void* mgmt_socket;
+
     mc_neighbour_s neighbours[MC_MAX_NEIGHBOURS];
     uint8_t neighbours_len;