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

PCM resolution delay

This commit is contained in:
emanuel 2022-02-22 15:52:34 +00:00
parent 3be2a9d2f8
commit 89943a2789
2 changed files with 27 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
src/pcm.c
View File

@ -48,7 +48,7 @@ static int are_vehicles_intersecting(
return 0; return 0;
} }
static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm) { static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_neighbour_s* neighbour) {
int rv = 0; int rv = 0;
CoordinationRequest_t* request = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.request; CoordinationRequest_t* request = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.request;
@ -64,6 +64,9 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm) {
uint64_t now = it2s_tender_get_clock(&facilities->epv); uint64_t now = it2s_tender_get_clock(&facilities->epv);
neighbour->proposed = true;
neighbour->t_proposal = now;
int32_t lat, lon; int32_t lat, lon;
it2s_tender_lock_space(&facilities->epv); it2s_tender_lock_space(&facilities->epv);
it2s_tender_get_space(&facilities->epv); it2s_tender_get_space(&facilities->epv);
@ -165,7 +168,7 @@ cleanup:
return rv; return rv;
} }
static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm) { static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm, mc_neighbour_s* neighbour) {
int rv = 0; int rv = 0;
CoordinationReply_t* reply = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.reply; CoordinationReply_t* reply = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.reply;
@ -175,7 +178,7 @@ static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm) {
return rv; return rv;
} }
static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm) { static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm, mc_neighbour_s* neighbour) {
int rv = 0; int rv = 0;
@ -188,6 +191,21 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm)
it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
uint16_t trajectoryA_len = 0; uint16_t trajectoryA_len = 0;
uint64_t now = it2s_tender_get_clock(&facilities->epv);
if (now < neighbour->t_iid + MC_RESOLUTION_TIMEOUT) {
rv = 0;
goto cleanup;
}
neighbour->intersecting = true;
neighbour->t_iid = now;
if (now < neighbour->t_proposal + MC_RESOLUTION_TIMEOUT) {
rv = 0;
goto cleanup;
}
int32_t lat, lon; int32_t lat, lon;
it2s_tender_lock_space(&facilities->epv); it2s_tender_lock_space(&facilities->epv);
it2s_tender_get_space(&facilities->epv); it2s_tender_get_space(&facilities->epv);
@ -200,7 +218,6 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm)
memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s)); memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s));
it2s_tender_unlock_trajectory(&facilities->epv); it2s_tender_unlock_trajectory(&facilities->epv);
uint64_t now = it2s_tender_get_clock(&facilities->epv);
trajectoryA[0].latitude = lat; trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon; trajectoryA[0].longitude = lon;
@ -356,10 +373,10 @@ int pcm_check(void* fc, PCM_t* pcm) {
if (mvc->negotiation) { if (mvc->negotiation) {
switch (mvc->negotiation->present) { switch (mvc->negotiation->present) {
case CoordinationNegotiation_PR_request: case CoordinationNegotiation_PR_request:
pcm_check_handle_request(facilities, pcm); pcm_check_handle_request(facilities, pcm, &coordination->neighbours[ni]);
break; break;
case CoordinationNegotiation_PR_reply: case CoordinationNegotiation_PR_reply:
pcm_check_handle_reply(facilities, pcm); pcm_check_handle_reply(facilities, pcm, &coordination->neighbours[ni]);
break; break;
default: default:
break; break;
@ -400,9 +417,7 @@ int pcm_check(void* fc, PCM_t* pcm) {
if (intersecting) { if (intersecting) {
syslog_info("[facilities] [pc] intersection danger with %ld @ (%f, %f) in %ld ms", syslog_info("[facilities] [pc] intersection danger with %ld @ (%f, %f) in %ld ms",
pcm->header.stationID, trajectoryA[index].latitude/1.0e7, trajectoryA[index].longitude/1.0e7, trajectoryA[index].timestamp - now); pcm->header.stationID, trajectoryA[index].latitude/1.0e7, trajectoryA[index].longitude/1.0e7, trajectoryA[index].timestamp - now);
coordination->neighbours[ni].intersecting = true; pcm_check_intersection_detected(facilities, pcm, &coordination->neighbours[ni]);
coordination->neighbours[ni].t_iid = now;
pcm_check_intersection_detected(facilities, pcm);
} }
} }
break; break;

3
src/pcm.h
View File

@ -11,6 +11,9 @@ typedef struct {
bool intersecting; bool intersecting;
uint64_t t_iid; /* initial intersection detection */ uint64_t t_iid; /* initial intersection detection */
bool proposed;
uint64_t t_proposal;
} mc_neighbour_s; } mc_neighbour_s;
typedef struct coordination { typedef struct coordination {