diff --git a/src/dcm.c b/src/dcm.c index e85046e..320dd74 100644 --- a/src/dcm.c +++ b/src/dcm.c @@ -1,6 +1,7 @@ #include "dcm.h" #include "facilities.h" +#include #include #include #include @@ -51,7 +52,13 @@ static int are_vehicles_intersecting( int dcm_check(void* fc, DCM_t* dcm) { facilities_t* facilities = (facilities_t*) fc; + int rv = 0; + 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 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); // Initiate conflict resolution - dcm = calloc(1, sizeof(DCM_t)); + dcm_req = calloc(1, sizeof(DCM_t)); it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN]; - dcm->header.messageID = 43; - dcm->header.protocolVersion = 1; - dcm->header.stationID = 1; + dcm_req->header.messageID = 43; + dcm_req->header.protocolVersion = 1; + dcm_req->header.stationID = 1; 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); - dcm->dcm.currentPosition.latitude = lat; - dcm->dcm.currentPosition.longitude = lon; - asn_ulong2INTEGER(&dcm->dcm.currentPosition.timestamp, now); + dcm_req->dcm.currentPosition.latitude = lat; + dcm_req->dcm.currentPosition.longitude = lon; + asn_ulong2INTEGER(&dcm_req->dcm.currentPosition.timestamp, now); // Vehicle Dimensions - dcm->dcm.vehicleDimensions.length= facilities->vehicle.length; - dcm->dcm.vehicleDimensions.width = facilities->vehicle.width; + dcm_req->dcm.vehicleDimensions.length= facilities->vehicle.length; + dcm_req->dcm.vehicleDimensions.width = facilities->vehicle.width; // Planned trajectory - dcm->dcm.plannedTrajectory.list.count = --trajectoryA_len; - dcm->dcm.plannedTrajectory.list.size = trajectoryA_len * sizeof(void*); - dcm->dcm.plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*)); + dcm_req->dcm.plannedTrajectory.list.count = --trajectoryA_len; + dcm_req->dcm.plannedTrajectory.list.size = trajectoryA_len * sizeof(void*); + dcm_req->dcm.plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*)); for (int i = 0; i < trajectoryA_len; ++i) { - dcm->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); - dcm->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i+1].latitude; - dcm->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i+1].longitude; - asn_ulong2INTEGER(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i+1].timestamp); + dcm_req->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); + dcm_req->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i+1].latitude; + dcm_req->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i+1].longitude; + asn_ulong2INTEGER(&dcm_req->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i+1].timestamp); } // Desired trajectory - dcm->dcm.negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); - dcm->dcm.negotiation->present = CoordinationNegotiation_PR_request; - dcm->dcm.negotiation->choice.request.desiredTrajectories.list.count = 1; - dcm->dcm.negotiation->choice.request.desiredTrajectories.list.size = sizeof(void*); - dcm->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)); - ProposedTrajectory_t* pt = dcm->dcm.negotiation->choice.request.desiredTrajectories.list.array[0]; + dcm_req->dcm.negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); + dcm_req->dcm.negotiation->present = CoordinationNegotiation_PR_request; + dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.count = 1; + dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.size = sizeof(void*); + dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.array = malloc(1*sizeof(void*)); + dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t)); + ProposedTrajectory_t* pt = dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.array[0]; pt->trajectory.list.count = 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->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: 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) {