TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

VCM: Speed change on VCM req rx

This commit is contained in:
emanuel 2022-10-21 11:58:29 +01:00
parent 5a91508050
commit 42cee38184
3 changed files with 47 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/facilities.c
View File

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

54
src/vcm.c
View File

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

2
src/vcm.h
View File

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