From fee7bd59a762dfd07dba88018d458df2326d80ee Mon Sep 17 00:00:00 2001 From: emanuel Date: Tue, 22 Feb 2022 10:12:54 +0000 Subject: [PATCH] Changed DCM to PCM --- src/CMakeLists.txt | 4 +- src/facilities.c | 10 +- src/facilities.h | 4 +- src/{dcm.c => pcm.c} | 314 +++++++++++++++++++++++-------------------- src/{dcm.h => pcm.h} | 6 +- 5 files changed, 181 insertions(+), 157 deletions(-) rename src/{dcm.c => pcm.c} (53%) rename src/{dcm.h => pcm.h} (84%) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 6d7855d..71ed6f1 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -9,7 +9,7 @@ ADD_EXECUTABLE(it2s-itss-facilities cpm.c saem.c tpm.c - dcm.c + pcm.c ) TARGET_LINK_LIBRARIES(it2s-itss-facilities @@ -27,7 +27,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities -lit2s-asn-cpm -lit2s-asn-saem -lit2s-asn-tpm - -lit2s-asn-dcm + -lit2s-asn-pcm -lit2s-tender -lm -lrt diff --git a/src/facilities.c b/src/facilities.c index 950ad8f..4287719 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -6,7 +6,7 @@ #include "requests.h" #include "cpm.h" #include "saem.h" -#include "dcm.h" +#include "pcm.h" #include #include @@ -123,8 +123,8 @@ static int transport_indication(facilities_t *facilities, void* responder, void* break; case 2043: /* maneuvers */ - its_msg_descriptor = &asn_DEF_DCM; - its_msg = calloc(1, sizeof(DCM_t)); + its_msg_descriptor = &asn_DEF_PCM; + its_msg = calloc(1, sizeof(PCM_t)); handled_msg = true; break; @@ -223,7 +223,7 @@ static int transport_indication(facilities_t *facilities, void* responder, void* break; case 2043: - dcm_check(facilities, its_msg); + pcm_check(facilities, its_msg); break; default: @@ -710,7 +710,7 @@ int main() { // DC if (facilities.coordination.active) - pthread_create(&facilities.dc_service, NULL, dc_service, (void*) &facilities); + pthread_create(&facilities.pc_service, NULL, pc_service, (void*) &facilities); void* security_socket = zmq_socket(facilities.zmq.ctx, ZMQ_REQ); int wait_ms = 1000; diff --git a/src/facilities.h b/src/facilities.h index 5a24e68..a10120c 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -12,7 +12,7 @@ #include "cpm.h" #include "saem.h" #include "tpm.h" -#include "dcm.h" +#include "pcm.h" #include #include @@ -31,7 +31,7 @@ typedef struct facilities { pthread_t transmitting; pthread_t cp_service; pthread_t sa_service; - pthread_t dc_service; + pthread_t pc_service; // ZMQ struct { diff --git a/src/dcm.c b/src/pcm.c similarity index 53% rename from src/dcm.c rename to src/pcm.c index ee711f2..686e310 100644 --- a/src/dcm.c +++ b/src/pcm.c @@ -1,13 +1,12 @@ -#include "dcm.h" +#include "pcm.h" #include "facilities.h" -#include #include #include #include #include #include -#include +#include #define SLEEP_TIME_MS 100 @@ -49,12 +48,12 @@ static int are_vehicles_intersecting( return 0; } -static int dcm_check_handle_request(facilities_t* facilities, DCM_t* dcm) { +static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm) { int rv = 0; - CoordinationRequest_t* coo_req = &dcm->dcm.negotiation->choice.request; + CoordinationRequest_t* request = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.request; - DCM_t* dcm_rep = NULL; + PCM_t* pcm_rep = NULL; TransportRequest_t* tr = NULL; const ssize_t buf_len = 512; @@ -82,43 +81,46 @@ static int dcm_check_handle_request(facilities_t* facilities, DCM_t* dcm) { trajectoryA[0].timestamp = now; ++trajectoryA_len; - dcm_rep = calloc(1, sizeof(DCM_t)); + pcm_rep = calloc(1, sizeof(PCM_t)); - dcm_rep->header.messageID = 43; - dcm_rep->header.protocolVersion = 1; - dcm_rep->header.stationID = 1; + pcm_rep->header.messageID = 43; + pcm_rep->header.protocolVersion = 1; + pcm_rep->header.stationID = 1; pthread_mutex_lock(&facilities->id.lock); - dcm_rep->header.stationID = facilities->id.station_id; + pcm_rep->header.stationID = facilities->id.station_id; pthread_mutex_unlock(&facilities->id.lock); - dcm_rep->dcm.currentPosition.latitude = lat; - dcm_rep->dcm.currentPosition.longitude = lon; - asn_ulong2INTEGER(&dcm_rep->dcm.currentPosition.timestamp, now); + pcm_rep->pcm.currentPosition.latitude = lat; + pcm_rep->pcm.currentPosition.longitude = lon; + asn_ulong2INTEGER(&pcm_rep->pcm.currentPosition.timestamp, now); + + pcm_rep->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; + ManeuverVehicleContainer_t* mvc = &pcm_rep->pcm.maneuverContainer.choice.vehicle; // Vehicle DimensionsDimensions - dcm_rep->dcm.vehicleLength.vehicleLengthValue= facilities->vehicle.length; - dcm_rep->dcm.vehicleLength.vehicleLengthConfidenceIndication = 0; - dcm_rep->dcm.vehicleWidth = facilities->vehicle.width; + mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length; + mvc->vehicleLength.vehicleLengthConfidenceIndication = 0; + mvc->vehicleWidth = facilities->vehicle.width; // Accepted trajectory - if (!coo_req->desiredTrajectories.list.count) { - syslog_debug("[facilities] [dc] received DCM request has no desired trajectories"); + if (!request->desiredTrajectories.list.count) { + syslog_debug("[facilities] [pc] received PCM request has no desired trajectories"); rv = 1; goto cleanup; } - ProposedTrajectory_t* pt = coo_req->desiredTrajectories.list.array[0]; + ProposedTrajectory_t* pt = request->desiredTrajectories.list.array[0]; - dcm_rep->dcm.negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); - dcm_rep->dcm.negotiation->present = CoordinationNegotiation_PR_reply; - dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 1; - dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectoriesIds.list.size = sizeof(void*); - dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectoriesIds.list.array = malloc(1*sizeof(void*)); - dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = malloc(sizeof(long)); - *dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = pt->id; + mvc->negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); + mvc->negotiation->present = CoordinationNegotiation_PR_reply; + mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 1; + mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = sizeof(void*); + mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = malloc(1*sizeof(void*)); + mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = malloc(sizeof(long)); + *mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = pt->id; - asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_DCM, dcm_rep, buf, buf_len); + asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_PCM, pcm_rep, buf, buf_len); if (enc.encoded == -1) { - syslog_err("[facilities] [dc] DCM request encode failure"); + syslog_err("[facilities] [pc] PCM request encode failure"); rv = 1; goto cleanup; } @@ -149,7 +151,7 @@ static int dcm_check_handle_request(facilities_t* facilities, DCM_t* dcm) { 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"); + syslog_err("[facilities] [pc] TR PCM request encode failure"); rv = 1; goto cleanup; } @@ -157,26 +159,26 @@ static int dcm_check_handle_request(facilities_t* facilities, DCM_t* dcm) { queue_send(facilities->tx_queue, buf, enc.encoded+1, 3); cleanup: - ASN_STRUCT_FREE(asn_DEF_DCM, dcm_rep); + ASN_STRUCT_FREE(asn_DEF_PCM, pcm_rep); ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); return rv; } -static int dcm_check_handle_reply(facilities_t* facilities, DCM_t* dcm) { +static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm) { int rv = 0; - CoordinationReply_t* coo_rep = &dcm->dcm.negotiation->choice.reply; + CoordinationReply_t* reply = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.reply; - syslog_info("[facilities] [dc] received coordination reply with %d accepted trajectories", coo_rep->acceptedTrajectoriesIds.list.count); + syslog_info("[facilities] [pc] received coordination reply with %d accepted trajectories", reply->acceptedTrajectoriesIds.list.count); return rv; } -static int dcm_check_intersection_detected(facilities_t* facilities, DCM_t* dcm) { +static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm) { int rv = 0; - DCM_t* dcm_req = NULL; + PCM_t* pcm_req = NULL; TransportRequest_t* tr = NULL; const ssize_t buf_len = 512; @@ -205,43 +207,45 @@ static int dcm_check_intersection_detected(facilities_t* facilities, DCM_t* dcm) ++trajectoryA_len; // Initiate conflict resolution - dcm_req = calloc(1, sizeof(DCM_t)); + pcm_req = calloc(1, sizeof(PCM_t)); - dcm_req->header.messageID = 43; - dcm_req->header.protocolVersion = 1; - dcm_req->header.stationID = 1; + pcm_req->header.messageID = 43; + pcm_req->header.protocolVersion = 1; + pcm_req->header.stationID = 1; pthread_mutex_lock(&facilities->id.lock); - dcm_req->header.stationID = facilities->id.station_id; + pcm_req->header.stationID = facilities->id.station_id; pthread_mutex_unlock(&facilities->id.lock); - dcm_req->dcm.currentPosition.latitude = lat; - dcm_req->dcm.currentPosition.longitude = lon; - asn_ulong2INTEGER(&dcm_req->dcm.currentPosition.timestamp, now); + pcm_req->pcm.currentPosition.latitude = lat; + pcm_req->pcm.currentPosition.longitude = lon; + asn_ulong2INTEGER(&pcm_req->pcm.currentPosition.timestamp, now); + + ManeuverVehicleContainer_t* mvc = &pcm_req->pcm.maneuverContainer.choice.vehicle; // Vehicle Dimensions - dcm_req->dcm.vehicleLength.vehicleLengthValue= facilities->vehicle.length; - dcm_req->dcm.vehicleLength.vehicleLengthConfidenceIndication = 0; - dcm_req->dcm.vehicleWidth = facilities->vehicle.width; + mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length; + mvc->vehicleLength.vehicleLengthConfidenceIndication = 0; + mvc->vehicleWidth = facilities->vehicle.width; // Planned trajectory - 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*)); + mvc->plannedTrajectory.list.count = --trajectoryA_len; + mvc->plannedTrajectory.list.size = trajectoryA_len * sizeof(void*); + mvc->plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*)); for (int i = 0; i < trajectoryA_len; ++i) { - dcm_req->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); - dcm_req->dcm.plannedTrajectory.list.array[i]->latitude = trajectoryA[i+1].latitude; - dcm_req->dcm.plannedTrajectory.list.array[i]->longitude = trajectoryA[i+1].longitude; - asn_ulong2INTEGER(&dcm_req->dcm.plannedTrajectory.list.array[i]->timestamp, trajectoryA[i+1].timestamp); + mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); + mvc->plannedTrajectory.list.array[i]->latitude = trajectoryA[i+1].latitude; + mvc->plannedTrajectory.list.array[i]->longitude = trajectoryA[i+1].longitude; + asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[i]->timestamp, trajectoryA[i+1].timestamp); } // Desired trajectory - 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]; + mvc->negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); + mvc->negotiation->present = CoordinationNegotiation_PR_request; + mvc->negotiation->choice.request.desiredTrajectories.list.count = 1; + mvc->negotiation->choice.request.desiredTrajectories.list.size = sizeof(void*); + mvc->negotiation->choice.request.desiredTrajectories.list.array = malloc(1*sizeof(void*)); + mvc->negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t)); + ProposedTrajectory_t* pt = mvc->negotiation->choice.request.desiredTrajectories.list.array[0]; pt->trajectory.list.count = trajectoryA_len; pt->trajectory.list.size = sizeof(void*) * trajectoryA_len; @@ -257,9 +261,9 @@ static int dcm_check_intersection_detected(facilities_t* facilities, 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); + asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_PCM, pcm_req, buf, buf_len); if (enc.encoded == -1) { - syslog_err("[facilities] [dc] DCM request encode failure"); + syslog_err("[facilities] [pc] PCM request encode failure"); rv = 1; goto cleanup; } @@ -290,7 +294,7 @@ static int dcm_check_intersection_detected(facilities_t* facilities, DCM_t* dcm) 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"); + syslog_err("[facilities] [pc] TR PCM request encode failure"); rv = 1; goto cleanup; } @@ -298,12 +302,12 @@ static int dcm_check_intersection_detected(facilities_t* facilities, DCM_t* dcm) queue_send(facilities->tx_queue, buf, enc.encoded+1, 3); cleanup: - ASN_STRUCT_FREE(asn_DEF_DCM, dcm_req); + ASN_STRUCT_FREE(asn_DEF_PCM, pcm_req); ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); return rv; } -int dcm_check(void* fc, DCM_t* dcm) { +int pcm_check(void* fc, PCM_t* pcm) { facilities_t* facilities = (facilities_t*) fc; coordination_s* coordination = &facilities->coordination; @@ -323,36 +327,12 @@ int dcm_check(void* fc, DCM_t* dcm) { lon = facilities->epv.space.longitude; it2s_tender_unlock_space(&facilities->epv); - it2s_tender_lock_trajectory(&facilities->epv); - trajectoryA_len = facilities->epv.trajectory.len; - memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s)); - it2s_tender_unlock_trajectory(&facilities->epv); - - trajectoryA[0].latitude = lat; - trajectoryA[0].longitude = lon; - trajectoryA[0].timestamp = now; - ++trajectoryA_len; - - trajectoryB[0].latitude = dcm->dcm.currentPosition.latitude; - trajectoryB[0].longitude = dcm->dcm.currentPosition.longitude; - asn_INTEGER2ulong(&dcm->dcm.currentPosition.timestamp, &trajectoryB[0].timestamp); - ++trajectoryB_len; - for (int i = 0; i < dcm->dcm.plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) { - trajectoryB[i+1].latitude = dcm->dcm.plannedTrajectory.list.array[i]->latitude; - trajectoryB[i+1].longitude = dcm->dcm.plannedTrajectory.list.array[i]->longitude; - asn_INTEGER2ulong(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, &trajectoryB[i+1].timestamp); - ++trajectoryB_len; - } - - bool intersecting = false; - int index = 0; - pthread_mutex_lock(&coordination->lock); // Find neighbours int ni = -1; for (int i = 0; i < coordination->neighbours_len; ++i) { - if (coordination->neighbours->station_id == dcm->header.stationID) { + if (coordination->neighbours->station_id == pcm->header.stationID) { ni = i; break; } @@ -360,37 +340,73 @@ int dcm_check(void* fc, DCM_t* dcm) { // Add it if not found if (ni == -1 && coordination->neighbours_len + 1 < MC_MAX_NEIGHBOURS) { - coordination->neighbours[coordination->neighbours_len].station_id = dcm->header.stationID; + coordination->neighbours[coordination->neighbours_len].station_id = pcm->header.stationID; ni = coordination->neighbours_len; ++coordination->neighbours_len; } - if (dcm->dcm.negotiation) { - switch (dcm->dcm.negotiation->present) { - case CoordinationNegotiation_PR_request: - dcm_check_handle_request(facilities, dcm); - break; - case CoordinationNegotiation_PR_reply: - dcm_check_handle_reply(facilities, dcm); - break; - default: - break; - } - } else { - if (trajectoryA_len > 1 && trajectoryB_len > 1) { - intersecting = are_vehicles_intersecting( - trajectoryA, trajectoryA_len, facilities->vehicle.length, facilities->vehicle.width, - trajectoryB, trajectoryB_len, dcm->dcm.vehicleLength.vehicleLengthValue, dcm->dcm.vehicleWidth, - &index); - } + switch (pcm->pcm.maneuverContainer.present) { + case ManeuverContainer_PR_vehicle: + ; + ManeuverVehicleContainer_t* mvc = &pcm->pcm.maneuverContainer.choice.vehicle; + if (mvc->negotiation) { + switch (mvc->negotiation->present) { + case CoordinationNegotiation_PR_request: + pcm_check_handle_request(facilities, pcm); + break; + case CoordinationNegotiation_PR_reply: + pcm_check_handle_reply(facilities, pcm); + break; + default: + break; + } + } else { - if (intersecting) { - syslog_info("[facilities] [dc] intersection danger with %ld @ (%d, %d) in %ld ms", - dcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now); - coordination->neighbours[ni].intersecting = true; - coordination->neighbours[ni].t_iid = now; - dcm_check_intersection_detected(facilities, dcm); - } + it2s_tender_lock_trajectory(&facilities->epv); + trajectoryA_len = facilities->epv.trajectory.len; + memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s)); + it2s_tender_unlock_trajectory(&facilities->epv); + + trajectoryA[0].latitude = lat; + trajectoryA[0].longitude = lon; + trajectoryA[0].timestamp = now; + ++trajectoryA_len; + + trajectoryB[0].latitude = pcm->pcm.currentPosition.latitude; + trajectoryB[0].longitude = pcm->pcm.currentPosition.longitude; + asn_INTEGER2ulong(&pcm->pcm.currentPosition.timestamp, &trajectoryB[0].timestamp); + ++trajectoryB_len; + for (int i = 0; i < mvc->plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) { + trajectoryB[i+1].latitude = mvc->plannedTrajectory.list.array[i]->latitude; + trajectoryB[i+1].longitude = mvc->plannedTrajectory.list.array[i]->longitude; + asn_INTEGER2ulong(&mvc->plannedTrajectory.list.array[i]->timestamp, &trajectoryB[i+1].timestamp); + ++trajectoryB_len; + } + + bool intersecting = false; + int index = -1; + + if (trajectoryA_len > 1 && trajectoryB_len > 1) { + intersecting = are_vehicles_intersecting( + trajectoryA, trajectoryA_len, facilities->vehicle.length, facilities->vehicle.width, + trajectoryB, trajectoryB_len, mvc->vehicleLength.vehicleLengthValue, mvc->vehicleWidth, + &index); + } + + if (intersecting) { + syslog_info("[facilities] [pc] intersection danger with %ld @ (%d, %d) in %ld ms", + pcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now); + coordination->neighbours[ni].intersecting = true; + coordination->neighbours[ni].t_iid = now; + pcm_check_intersection_detected(facilities, pcm); + } + } + break; + case ManeuverContainer_PR_rsu: + break; + default: + syslog_debug("[facilities] [pc] received PCM contains unrecognized ManeuverContainer type"); + rv = 1; } pthread_mutex_unlock(&coordination->lock); @@ -398,18 +414,18 @@ int dcm_check(void* fc, DCM_t* dcm) { return rv; } -static int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len) { +static int mk_pcm(facilities_t* facilities, uint8_t* pcm_uper, uint16_t* pcm_uper_len) { int rv = 0; it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN]; - DCM_t* dcm = calloc(1, sizeof(DCM_t)); + PCM_t* pcm = calloc(1, sizeof(PCM_t)); - dcm->header.messageID = 43; - dcm->header.protocolVersion = 1; + pcm->header.messageID = 43; + pcm->header.protocolVersion = 1; uint64_t now = it2s_tender_get_clock(&facilities->epv); pthread_mutex_lock(&facilities->id.lock); - dcm->header.stationID = facilities->id.station_id; + pcm->header.stationID = facilities->id.station_id; pthread_mutex_unlock(&facilities->id.lock); int32_t lat, lon; @@ -425,45 +441,53 @@ static int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_upe memcpy(trajectory, facilities->epv.trajectory.path, trajectory_len * sizeof(it2s_tender_st_s)); it2s_tender_unlock_trajectory(&facilities->epv); - dcm->dcm.currentPosition.latitude = lat; - dcm->dcm.currentPosition.longitude = lon; - asn_ulong2INTEGER(&dcm->dcm.currentPosition.timestamp, now); + pcm->pcm.currentPosition.latitude = lat; + pcm->pcm.currentPosition.longitude = lon; + asn_ulong2INTEGER(&pcm->pcm.currentPosition.timestamp, now); - // Vehicle Dimensions - dcm->dcm.vehicleLength.vehicleLengthValue= facilities->vehicle.length; - dcm->dcm.vehicleLength.vehicleLengthConfidenceIndication = 0; - dcm->dcm.vehicleWidth = facilities->vehicle.width; + if (facilities->station_type == 15) { + pcm->pcm.maneuverContainer.present = ManeuverContainer_PR_rsu; + ManeuverRSUContainer_t* mrc = &pcm->pcm.maneuverContainer.choice.rsu; + mrc->recommendedTrajectories = NULL; // TODO + } else { + pcm->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; + ManeuverVehicleContainer_t* mvc = &pcm->pcm.maneuverContainer.choice.vehicle; + // Vehicle Dimensions + mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length; + mvc->vehicleLength.vehicleLengthConfidenceIndication = 0; + mvc->vehicleWidth = facilities->vehicle.width; - dcm->dcm.plannedTrajectory.list.count = trajectory_len; - dcm->dcm.plannedTrajectory.list.size = trajectory_len * sizeof(void*); - dcm->dcm.plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*)); - for (int i = 0; i < trajectory_len; ++i) { - dcm->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); - dcm->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i].latitude; - dcm->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i].longitude; - asn_ulong2INTEGER(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i].timestamp); + mvc->plannedTrajectory.list.count = trajectory_len; + mvc->plannedTrajectory.list.size = trajectory_len * sizeof(void*); + mvc->plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*)); + for (int i = 0; i < trajectory_len; ++i) { + mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); + mvc->plannedTrajectory.list.array[i]->latitude = trajectory[i].latitude; + mvc->plannedTrajectory.list.array[i]->longitude = trajectory[i].longitude; + asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[i]->timestamp, trajectory[i].timestamp); + } } - asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_DCM, NULL, dcm, dcm_uper, 512); + asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_PCM, NULL, pcm, pcm_uper, 512); if (enc.encoded == -1) { - syslog_err("[facilities] [dc] failed encoding DCM (%s)", enc.failed_type->name); + syslog_err("[facilities] [pc] failed encoding PCM (%s)", enc.failed_type->name); rv = 1; goto cleanup; } - *dcm_uper_len = (enc.encoded + 7) / 8; + *pcm_uper_len = (enc.encoded + 7) / 8; cleanup: - ASN_STRUCT_FREE(asn_DEF_DCM, dcm); + ASN_STRUCT_FREE(asn_DEF_PCM, pcm); return rv; } -void* dc_service(void* fc) { +void* pc_service(void* fc) { facilities_t* facilities = (facilities_t*) fc; coordination_s* coordination = (coordination_s*) &facilities->coordination; - uint8_t dcm[512]; + uint8_t pcm[512]; TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t)); tr->present = TransportRequest_PR_packet; @@ -493,14 +517,14 @@ void* dc_service(void* fc) { while (!facilities->exit) { - rv = mk_dcm(facilities, bpr->data.buf, (uint16_t *) &bpr->data.size); + rv = mk_pcm(facilities, bpr->data.buf, (uint16_t *) &bpr->data.size); if (rv) { continue; } asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 1023); if (enc.encoded == -1) { - syslog_err("[facilities] encoding TR for DCM failed"); + syslog_err("[facilities] encoding TR for PCM failed"); continue; } diff --git a/src/dcm.h b/src/pcm.h similarity index 84% rename from src/dcm.h rename to src/pcm.h index 8112956..c2c0449 100644 --- a/src/dcm.h +++ b/src/pcm.h @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #define MC_MAX_NEIGHBOURS 32 #define MC_RESOLUTION_TIMEOUT 1000 @@ -23,6 +23,6 @@ typedef struct coordination { } coordination_s; -int dcm_check(void* fc, DCM_t* dcm); -void* dc_service(void* fc); +int pcm_check(void* fc, PCM_t* pcm); +void* pc_service(void* fc); void coordination_init(coordination_s* coordination);