From 42cee381846b7e5cc66576e173c5c56860445ee5 Mon Sep 17 00:00:00 2001 From: emanuel Date: Fri, 21 Oct 2022 11:58:29 +0100 Subject: [PATCH] VCM: Speed change on VCM req rx --- src/facilities.c | 8 +++---- src/vcm.c | 54 ++++++++++++++++++++++++++++++++++++------------ src/vcm.h | 2 ++ 3 files changed, 47 insertions(+), 17 deletions(-) diff --git a/src/facilities.c b/src/facilities.c index 0025e61..f12261e 100644 --- a/src/facilities.c +++ b/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 - if (facilities.coordination.active) + // VC + 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); diff --git a/src/vcm.c b/src/vcm.c index 3cf9d82..cfa4647 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -9,6 +9,7 @@ #include #include #include +#include #include 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] "); + } + 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; - enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1); + buf1[0] = 4; + 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; - enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, buf_len-1); + buf1[0] = 4; + 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) { - pthread_mutex_init(&coordination->lock, NULL); +void coordination_init() { + coordination_t* coo = &facilities.coordination; + pthread_mutex_init(&coo->lock, NULL); + coo->mgmt_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ); } diff --git a/src/vcm.h b/src/vcm.h index 023e52b..0b4884b 100644 --- a/src/vcm.h +++ b/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;