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

PCM chain info

This commit is contained in:
emanuel 2022-03-14 14:29:53 +00:00
parent 322efce93c
commit 9799900d27
5 changed files with 76 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/facilities.c
View File

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

34
src/pcm.c
View File

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

2
src/pcm.h
View File

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

31
src/requests.c
View File

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

1
src/requests.h
View File

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