From 9799900d27eef8b98774288b51846cf6654eae0d Mon Sep 17 00:00:00 2001 From: emanuel Date: Mon, 14 Mar 2022 14:29:53 +0000 Subject: [PATCH] PCM chain info --- src/facilities.c | 14 +++++++++----- src/pcm.c | 34 ++++++++++++++++++++++++++++++++++ src/pcm.h | 2 +- src/requests.c | 31 +++++++++++++++++++++++++++++++ src/requests.h | 1 + 5 files changed, 76 insertions(+), 6 deletions(-) diff --git a/src/facilities.c b/src/facilities.c index 0f3ee11..1338080 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -409,6 +409,10 @@ static int facilities_request(facilities_t *facilities, void* responder, uint8_t rv = facilities_request_loaded_protected_zones(facilities, responder, fr); break; + case FacilitiesDataRequest_PR_chainInfoSet: + rv = facilities_request_chaininfo_set(facilities, responder, &fr->choice.data.choice.chainInfoSet); + break; + default: syslog_err("[facilities] unrecognized FDR type received (%d)", fr->choice.data.present); facilities_request_result_rejected(responder); @@ -790,11 +794,11 @@ int main() { rl = zmq_recv(facilities.zmq.responders[i].socket, buffer, ITSS_SDU_MAX_LEN, 0); switch (buffer[0]) { - case 2: + case ITSS_NETWORKING: networking_indication(&facilities, facilities.zmq.responders[i].socket, buffer+1, rl); break; - case 3: + case ITSS_TRANSPORT: in_idchange = true; pthread_mutex_lock(&facilities.id.change.lock); if (facilities.id.change.stage == ID_CHANGE_INACTIVE) { @@ -815,7 +819,7 @@ int main() { } break; - case 5: + case ITSS_APPLICATIONS: in_idchange = true; pthread_mutex_lock(&facilities.id.change.lock); if (facilities.id.change.stage == ID_CHANGE_INACTIVE) { @@ -837,11 +841,11 @@ int main() { break; - case 6: + case ITSS_MANAGEMENT: management_indication(&facilities, facilities.zmq.responders[i].socket, buffer+1, rl); break; - case 7: + case ITSS_SECURITY: security_indication(&facilities, facilities.zmq.responders[i].socket, buffer+1, rl); break; diff --git a/src/pcm.c b/src/pcm.c index 0be4147..0cc9201 100644 --- a/src/pcm.c +++ b/src/pcm.c @@ -51,6 +51,8 @@ static int are_vehicles_intersecting( static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_neighbour_s* neighbour) { int rv = 0; + coordination_s* coordination = &facilities->coordination; + CoordinationRequest_t* request = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.request; PCM_t* pcm_rep = NULL; @@ -98,6 +100,16 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei pcm_rep->pcm.currentPosition.longitude = lon; asn_ulong2INTEGER(&pcm_rep->pcm.currentPosition.timestamp, now); + pthread_mutex_lock(&facilities->coordination.lock); + if (coordination->chain.id) { + pcm_rep->pcm.chain = calloc(1, sizeof(ChainInformation_t)); + pcm_rep->pcm.chain->id = coordination->chain.id; + pcm_rep->pcm.chain->link.buf = malloc(8); + pcm_rep->pcm.chain->link.size = 8; + memcpy(pcm_rep->pcm.chain->link.buf, coordination->chain.link, 8); + } + pthread_mutex_unlock(&facilities->coordination.lock); + pcm_rep->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; ManeuverVehicleContainer_t* mvc = &pcm_rep->pcm.maneuverContainer.choice.vehicle; @@ -219,6 +231,8 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm, int rv = 0; + coordination_s* coordination = &facilities->coordination; + PCM_t* pcm_req = NULL; TransportRequest_t* tr = NULL; FacilitiesIndication_t* fi = NULL; @@ -280,6 +294,16 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm, pcm_req->pcm.currentPosition.longitude = lon; asn_ulong2INTEGER(&pcm_req->pcm.currentPosition.timestamp, now); + pthread_mutex_lock(&facilities->coordination.lock); + if (coordination->chain.id) { + pcm_req->pcm.chain = calloc(1, sizeof(ChainInformation_t)); + pcm_req->pcm.chain->id = coordination->chain.id; + pcm_req->pcm.chain->link.buf = malloc(8); + pcm_req->pcm.chain->link.size = 8; + memcpy(pcm_req->pcm.chain->link.buf, coordination->chain.link, 8); + } + + pthread_mutex_unlock(&facilities->coordination.lock); pcm_req->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; ManeuverVehicleContainer_t* mvc = &pcm_req->pcm.maneuverContainer.choice.vehicle; @@ -510,6 +534,8 @@ int pcm_check(void* fc, PCM_t* pcm) { static int mk_pcm(facilities_t* facilities, uint8_t* pcm_uper, uint16_t* pcm_uper_len) { int rv = 0; + coordination_s* coordination = &facilities->coordination; + it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN]; PCM_t* pcm = calloc(1, sizeof(PCM_t)); @@ -538,6 +564,14 @@ static int mk_pcm(facilities_t* facilities, uint8_t* pcm_uper, uint16_t* pcm_upe pcm->pcm.currentPosition.longitude = lon; asn_ulong2INTEGER(&pcm->pcm.currentPosition.timestamp, now); + if (coordination->chain.id) { + pcm->pcm.chain = calloc(1, sizeof(ChainInformation_t)); + pcm->pcm.chain->id = coordination->chain.id; + pcm->pcm.chain->link.buf = malloc(8); + pcm->pcm.chain->link.size = 8; + memcpy(pcm->pcm.chain->link.buf, coordination->chain.link, 8); + } + if (facilities->station_type == StationType_roadSideUnit) { pcm->pcm.maneuverContainer.present = ManeuverContainer_PR_rsu; ManeuverRSUContainer_t* mrc = &pcm->pcm.maneuverContainer.choice.rsu; diff --git a/src/pcm.h b/src/pcm.h index 7f1e92c..ead6a5a 100644 --- a/src/pcm.h +++ b/src/pcm.h @@ -32,7 +32,7 @@ typedef struct coordination { struct { uint64_t id; - uint8_t link[32]; + uint8_t link[8]; it2s_tender_region_t region; } chain; diff --git a/src/requests.c b/src/requests.c index 26e5e01..b86365b 100644 --- a/src/requests.c +++ b/src/requests.c @@ -525,3 +525,34 @@ cleanup: return rv; } + +int facilities_request_chaininfo_set(facilities_t* facilities, void* responder, ChainInformation_t* cis) { + int rv = 0; + + pthread_mutex_lock(&facilities->coordination.lock); + facilities->coordination.chain.id = cis->id; + memcpy(facilities->coordination.chain.link, cis->link.buf, 8); + // TODO region + pthread_mutex_unlock(&facilities->coordination.lock); + + FacilitiesReply_t* frep = calloc(1, sizeof(FacilitiesReply_t)); + frep->present = FacilitiesReply_PR_data; + frep->choice.data.present = FacilitiesDataReply_PR_chainInfoSet; + frep->choice.data.choice.chainInfoSet = FacilitiesResultCode_accepted; + + uint8_t frep_oer[1024]; + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, frep, frep_oer, 1024); + if (enc.encoded == -1) { + syslog_err("[facilities] failed encoding FDResult (%s)", enc.failed_type->name); + facilities_request_result_rejected(responder); + rv = 1; + goto cleanup; + } + + zmq_send(responder, frep_oer, enc.encoded, 0); + +cleanup: + ASN_STRUCT_FREE(asn_DEF_FacilitiesReply, frep); + + return rv; +} diff --git a/src/requests.h b/src/requests.h index b636e10..7077386 100644 --- a/src/requests.h +++ b/src/requests.h @@ -11,5 +11,6 @@ int facilities_request_single_message(facilities_t* facilities, void* responder, int facilities_request_active_episodes(facilities_t* facilities, void* responder, FacilitiesRequest_t* fr); int facilities_request_attribute_types(facilities_t* facilities, void* responder, FacilitiesRequest_t* fr); int facilities_request_loaded_protected_zones(facilities_t* facilities, void* responder, FacilitiesRequest_t* fr); +int facilities_request_chaininfo_set(facilities_t* facilities, void* responder, ChainInformation_t* cis); #endif