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

Send DCM request on intersection

This commit is contained in:
emanuel 2022-02-14 16:54:35 +00:00
parent 16c8f385e1
commit 158da9f831
1 changed files with 73 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
src/dcm.c
View File

@ -1,6 +1,7 @@
#include "dcm.h" #include "dcm.h"
#include "facilities.h" #include "facilities.h"
#include <dcm/asn_application.h>
#include <it2s-tender/epv.h> #include <it2s-tender/epv.h>
#include <it2s-tender/space.h> #include <it2s-tender/space.h>
#include <it2s-tender/trajectory.h> #include <it2s-tender/trajectory.h>
@ -51,7 +52,13 @@ static int are_vehicles_intersecting(
int dcm_check(void* fc, DCM_t* dcm) { int dcm_check(void* fc, DCM_t* dcm) {
facilities_t* facilities = (facilities_t*) fc; facilities_t* facilities = (facilities_t*) fc;
int rv = 0;
DCM_t* dcm_req = NULL; DCM_t* dcm_req = NULL;
TransportRequest_t* tr = NULL;
const ssize_t buf_len = 512;
uint8_t buf[buf_len];
it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
it2s_tender_st_s trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */ it2s_tender_st_s trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */
@ -104,43 +111,43 @@ int dcm_check(void* fc, DCM_t* dcm) {
dcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now); dcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now);
// Initiate conflict resolution // Initiate conflict resolution
dcm = calloc(1, sizeof(DCM_t)); dcm_req = calloc(1, sizeof(DCM_t));
it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN]; it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN];
dcm->header.messageID = 43; dcm_req->header.messageID = 43;
dcm->header.protocolVersion = 1; dcm_req->header.protocolVersion = 1;
dcm->header.stationID = 1; dcm_req->header.stationID = 1;
pthread_mutex_lock(&facilities->id.lock); pthread_mutex_lock(&facilities->id.lock);
dcm->header.stationID = facilities->id.station_id; dcm_req->header.stationID = facilities->id.station_id;
pthread_mutex_unlock(&facilities->id.lock); pthread_mutex_unlock(&facilities->id.lock);
dcm->dcm.currentPosition.latitude = lat; dcm_req->dcm.currentPosition.latitude = lat;
dcm->dcm.currentPosition.longitude = lon; dcm_req->dcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&dcm->dcm.currentPosition.timestamp, now); asn_ulong2INTEGER(&dcm_req->dcm.currentPosition.timestamp, now);
// Vehicle Dimensions // Vehicle Dimensions
dcm->dcm.vehicleDimensions.length= facilities->vehicle.length; dcm_req->dcm.vehicleDimensions.length= facilities->vehicle.length;
dcm->dcm.vehicleDimensions.width = facilities->vehicle.width; dcm_req->dcm.vehicleDimensions.width = facilities->vehicle.width;
// Planned trajectory // Planned trajectory
dcm->dcm.plannedTrajectory.list.count = --trajectoryA_len; dcm_req->dcm.plannedTrajectory.list.count = --trajectoryA_len;
dcm->dcm.plannedTrajectory.list.size = trajectoryA_len * sizeof(void*); dcm_req->dcm.plannedTrajectory.list.size = trajectoryA_len * sizeof(void*);
dcm->dcm.plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*)); dcm_req->dcm.plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*));
for (int i = 0; i < trajectoryA_len; ++i) { for (int i = 0; i < trajectoryA_len; ++i) {
dcm->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); dcm_req->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
dcm->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i+1].latitude; dcm_req->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i+1].latitude;
dcm->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i+1].longitude; dcm_req->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i+1].longitude;
asn_ulong2INTEGER(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i+1].timestamp); asn_ulong2INTEGER(&dcm_req->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i+1].timestamp);
} }
// Desired trajectory // Desired trajectory
dcm->dcm.negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); dcm_req->dcm.negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
dcm->dcm.negotiation->present = CoordinationNegotiation_PR_request; dcm_req->dcm.negotiation->present = CoordinationNegotiation_PR_request;
dcm->dcm.negotiation->choice.request.desiredTrajectories.list.count = 1; dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.count = 1;
dcm->dcm.negotiation->choice.request.desiredTrajectories.list.size = sizeof(void*); dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.size = sizeof(void*);
dcm->dcm.negotiation->choice.request.desiredTrajectories.list.array = malloc(1*sizeof(void*)); dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.array = malloc(1*sizeof(void*));
dcm->dcm.negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t)); dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t));
ProposedTrajectory_t* pt = dcm->dcm.negotiation->choice.request.desiredTrajectories.list.array[0]; ProposedTrajectory_t* pt = dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.array[0];
pt->trajectory.list.count = trajectoryA_len; pt->trajectory.list.count = trajectoryA_len;
pt->trajectory.list.size = sizeof(void*) * trajectoryA_len; pt->trajectory.list.size = sizeof(void*) * trajectoryA_len;
@ -156,9 +163,50 @@ int dcm_check(void* fc, DCM_t* dcm) {
pt->priority = 1; pt->priority = 1;
pt->id = 0; pt->id = 0;
asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_DCM, dcm_req, buf, buf_len);
if (enc.encoded == -1) {
syslog_err("[facilities] [dc] DCM request encode failure");
rv = 1;
goto cleanup;
}
tr = calloc(1, sizeof(TransportRequest_t));
tr->present = TransportRequest_PR_packet;
tr->choice.packet.present = TransportPacketRequest_PR_btp;
BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;
bpr->btpType = BTPType_btpB;
bpr->gn.destinationAddress.buf = malloc(6);
for (int i = 0; i < 6; ++i) {
bpr->gn.destinationAddress.buf[i] = 0xff;
}
bpr->gn.destinationAddress.size = 6;
bpr->gn.packetTransportType = PacketTransportType_shb;
bpr->destinationPort = 2043;
bpr->gn.trafficClass = 2;
bpr->data.buf = malloc(512);
memcpy(bpr->data.buf, buf, buf_len);
bpr->data.size = (enc.encoded + 7) / 8;
buf[0] = 4;
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1);
if (enc.encoded == -1) {
syslog_err("[facilities] [dc] TR DCM request encode failure");
rv = 1;
goto cleanup;
}
queue_send(facilities->tx_queue, buf, enc.encoded+1, 3);
cleanup: cleanup:
ASN_STRUCT_FREE(asn_DEF_DCM, dcm_req); ASN_STRUCT_FREE(asn_DEF_DCM, dcm_req);
return 0; ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
return rv;
} }
static int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len) { static int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len) {