diff --git a/src/vcm.c b/src/vcm.c index a238961..8906aba 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -46,6 +46,29 @@ static int do_paths_intersect( return 0; } +static mc_neighbour_s* get_neighbour(uint32_t station_id) { + coordination_t* coordination = &facilities.coordination; + + mc_neighbour_s* neighbour = NULL; + + // Find neighbours + for (int i = 0; i < coordination->neighbours_len; ++i) { + if (coordination->neighbours[i].station_id == station_id) { + return &coordination->neighbours[i]; + } + } + + // Add it if not found + if (coordination->neighbours_len < MC_MAX_NEIGHBOURS) { + coordination->neighbours[coordination->neighbours_len].station_id = station_id; + ++coordination->neighbours_len; + return &coordination->neighbours[coordination->neighbours_len-1]; + } + + return NULL; +} + + static void tx_vcm(VCM_t* vcm) { const uint16_t buf_len = 2048; uint8_t buf[buf_len]; @@ -220,6 +243,110 @@ cleanup: ASN_STRUCT_FREE(asn_DEF_VCM, vcm_rep); } +static bool is_maneuver_approved() { + coordination_t* coordination = &facilities.coordination; + bool all_rep = true; + for (int t = 0; t < coordination->session.n_affs_trjs; ++t) { + if (coordination->session.n_affs_neighs[t] != 0) { + all_rep = false; + break; + } + } + if (all_rep) { + return true; + } + return false; +} + +static void session_cleanup() { + coordination_t* coordination = &facilities.coordination; + ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.own_req); + coordination->session.own_req = NULL; + coordination->session.ts = itss_time_get(); + coordination->session.nonce = 0; + coordination->session.requester = NULL; + memset(coordination->session.affs, 0, sizeof(coordination->session.affs)); + memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs)); + coordination->session.n_affs_trjs = 0; +} + +static bool commit() { + coordination_t* coordination = &facilities.coordination; + + if (coordination->protocol == MC_PROTOCOL_REQ_REP) { + return false; + } + + if (!is_maneuver_approved()) { + return false; + } + + VCM_t* vcm_com = calloc(1, sizeof(VCM_t)); + vcm_com->header.messageID = 43; + vcm_com->header.protocolVersion = 1; + pthread_mutex_lock(&facilities.id.lock); + vcm_com->header.stationID = facilities.id.station_id; + pthread_mutex_unlock(&facilities.id.lock); + + int32_t lat, lon; + itss_space_lock(); + itss_space_get(); + lat = epv.space.latitude; + lon = epv.space.longitude; + itss_space_unlock(); + vcm_com->vcm.currentPosition.latitude = lat; + vcm_com->vcm.currentPosition.longitude = lon; + + uint64_t now = itss_time_get(); + asn_ulong2INTEGER(&vcm_com->vcm.currentPosition.timestamp, now); + + vcm_com->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; + ManeuverVehicleContainer_t* mvc = &vcm_com->vcm.maneuverContainer.choice.vehicle; + + // Vehicle Dimensions + mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length; + mvc->vehicleLength.vehicleLengthConfidenceIndication = 0; + mvc->vehicleWidth = facilities.vehicle.width; + + itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ + uint16_t trajectoryA_len = 0; + + itss_trajectory_lock(); + trajectoryA_len = epv.trajectory.len; + memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t)); + itss_trajectory_unlock(); + + trajectoryA[0].latitude = lat; + trajectoryA[0].longitude = lon; + trajectoryA[0].timestamp = now; + ++trajectoryA_len; + + mvc->plannedTrajectory.list.count = trajectoryA_len - 1; + mvc->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*); + mvc->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*)); + for (int i = 0; i < trajectoryA_len - 1; ++i) { + mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); + mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude; + mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude; + mvc->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp; + } + + mvc->negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); + mvc->negotiation->present = CoordinationNegotiation_PR_commit; + mvc->negotiation->choice.commit.nonce = coordination->session.nonce; + mvc->negotiation->choice.commit.requesterId = coordination->session.own_req ? + vcm_com->header.stationID : coordination->session.requester->station_id; + + tx_vcm(vcm_com); + ASN_STRUCT_FREE(asn_DEF_VCM, vcm_com); + log_warn("SENDING COMMIT"); + + session_cleanup(); + + return true; +}; + + static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) { int rv = 0; @@ -231,7 +358,7 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) { uint64_t now = itss_time_get(); - if (coordination->session.req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) { /* in maneuver */ + if (coordination->session.own_req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) { /* in maneuver */ log_debug("[vc] rejecting VCM from %d - currently in maneuver", vcm->header.stationID); vcm_reject(vcm, neighbour); return 1; @@ -246,7 +373,7 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) { if (my_station_id == *request->desiredTrajectories.list.array[i]->affectingStations.list.array[j]) { is_req4me = true; } - coordination->session.affs[i][j] = *request->desiredTrajectories.list.array[i]->affectingStations.list.array[j]; + coordination->session.affs[i][j] = get_neighbour(*request->desiredTrajectories.list.array[i]->affectingStations.list.array[j]); } coordination->session.n_affs_neighs[i] = request->desiredTrajectories.list.array[i]->affectingStations.list.count; } @@ -321,12 +448,12 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) { itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ ssize_t trajectoryA_len = 0; - neighbour->t_proposal = now; neighbour->proposed = true; neighbour->session.requested = true; neighbour->session.nonce = request->nonce; neighbour->station_id = vcm->header.stationID; coordination->session.requester = neighbour; + coordination->session.t_init = now; vcm_rep = calloc(1, sizeof(VCM_t)); @@ -415,31 +542,10 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) { tx_vcm(vcm_rep); -cleanup: - ASN_STRUCT_FREE(asn_DEF_VCM, vcm_rep); - return rv; -} - -static bool is_maneuver_approved(CoordinationReply_t* reply, mc_neighbour_s* neighbour) { - coordination_t* coordination = &facilities.coordination; - if (reply->acceptedTrajectoriesIds.list.count == 0) { - ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req); - coordination->session.req = NULL; - coordination->session.ts = itss_time_get(); - memset(coordination->session.affs, 0, sizeof(coordination->session.affs)); - memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs)); - coordination->session.n_affs_trjs = 0; - neighbour->intersecting = false; - neighbour->proposed = false; - return false; - } - for (int t = 0; t < reply->acceptedTrajectoriesIds.list.count; ++t) { - int tid = *reply->acceptedTrajectoriesIds.list.array[t]; - if (tid > MC_TRAJECTORIES_N_MAX - 1) { - return false; - } + for (int t = 0; t < mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count; ++t) { + int tid = *mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[t]; for (int n = 0; n < coordination->session.n_affs_neighs[tid]; ++n) { - if (neighbour->station_id == coordination->session.affs[tid][n]) { + if (my_station_id == coordination->session.affs[tid][n]->station_id) { for (int i = n; i < coordination->session.n_affs_neighs[tid] - 1; ++i) { coordination->session.affs[tid][i] = coordination->session.affs[tid][i+1]; } @@ -448,25 +554,13 @@ static bool is_maneuver_approved(CoordinationReply_t* reply, mc_neighbour_s* nei } } } - bool all_rep = true; - for (int t = 0; t < coordination->session.n_affs_trjs; ++t) { - if (coordination->session.n_affs_neighs[t] != 0) { - all_rep = false; - break; - } - } - if (all_rep) { - ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req); - coordination->session.req = NULL; - coordination->session.ts = itss_time_get(); - memset(coordination->session.affs, 0, sizeof(coordination->session.affs)); - memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs)); - coordination->session.n_affs_trjs = 0; - return true; - } - return false; -} + commit(); + +cleanup: + ASN_STRUCT_FREE(asn_DEF_VCM, vcm_rep); + return rv; +} static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) { int rv = 0; @@ -479,19 +573,41 @@ static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) { uint64_t now_us = itss_ts_get(TIME_MICROSECONDS); itss_time_unlock(); - bool send_commit = false; - if (neighbour->intersecting) { /* REQUESTER */ - if (!coordination->session.req) { + if (!coordination->session.own_req) { log_debug("[vc] unknown reply context - session expired or another ITS-S rejected"); return 1; } - if (!is_maneuver_approved(reply, neighbour)) { + if (reply->acceptedTrajectoriesIds.list.count == 0) { + session_cleanup(); return 1; } + for (int t = 0; t < reply->acceptedTrajectoriesIds.list.count; ++t) { + if (t > MC_TRAJECTORIES_N_MAX - 1) { + return 1; + } + for (int n = 0; n < coordination->session.n_affs_neighs[t]; ++n) { + if (neighbour->station_id == coordination->session.affs[t][n]->station_id) { + for (int i = n; i < coordination->session.n_affs_neighs[t] - 1; ++i) { + coordination->session.affs[t][i] = coordination->session.affs[t][i+1]; + } + --coordination->session.n_affs_neighs[t]; + break; + } + } + } + + if (!is_maneuver_approved()) { + return 1; + } + + if (coordination->protocol == MC_PROTOCOL_REQ_REP) { + session_cleanup(); + } + log_info("[vc] received VCM.reply from %d with %d accepted trajectories | took %ld us", vcm->header.stationID, reply->acceptedTrajectoriesIds.list.count, @@ -500,93 +616,47 @@ static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) { neighbour->intersecting = false; neighbour->proposed = false; - if (coordination->protocol == MC_PROTOCOL_REQ_REP_COM) { - send_commit = true; - } - } else { /* REPLIER */ log_debug("[vc] received VCM.reply is response to another ITS-S VCM.request"); - if (coordination->protocol == MC_PROTOCOL_REQ_REP) { - return 1; - } - - /* Reply not meant for current session */ if (reply->nonce != coordination->session.nonce) { return 1; } - if (!coordination->session.requester) { + if (reply->acceptedTrajectoriesIds.list.count == 0) { + session_cleanup(); return 1; } - if (!is_maneuver_approved(reply, coordination->session.requester)) { - send_commit = true; + for (int t = 0; t < reply->acceptedTrajectoriesIds.list.count; ++t) { + int tid = *reply->acceptedTrajectoriesIds.list.array[t]; + if (tid > MC_TRAJECTORIES_N_MAX - 1) { + return 1; + } + for (int n = 0; n < coordination->session.n_affs_neighs[tid]; ++n) { + if (neighbour->station_id == coordination->session.affs[tid][n]->station_id) { + for (int i = n; i < coordination->session.n_affs_neighs[tid] - 1; ++i) { + coordination->session.affs[tid][i] = coordination->session.affs[tid][i+1]; + } + --coordination->session.n_affs_neighs[tid]; + break; + } + } + } + + if (!is_maneuver_approved()) { + return 1; + } + + if (coordination->protocol == MC_PROTOCOL_REQ_REP) { + session_cleanup(); } } - if (send_commit) { - VCM_t* vcm_com = calloc(1, sizeof(VCM_t)); - vcm->header.messageID = 43; - vcm->header.protocolVersion = 1; - pthread_mutex_lock(&facilities.id.lock); - vcm->header.stationID = facilities.id.station_id; - pthread_mutex_unlock(&facilities.id.lock); - - int32_t lat, lon; - itss_space_lock(); - itss_space_get(); - lat = epv.space.latitude; - lon = epv.space.longitude; - itss_space_unlock(); - vcm_com->vcm.currentPosition.latitude = lat; - vcm_com->vcm.currentPosition.longitude = lon; - - uint64_t now = itss_time_get(); - asn_ulong2INTEGER(&vcm_com->vcm.currentPosition.timestamp, now); - - vcm_com->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; - ManeuverVehicleContainer_t* mvc = &vcm_com->vcm.maneuverContainer.choice.vehicle; - - // Vehicle Dimensions - mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length; - mvc->vehicleLength.vehicleLengthConfidenceIndication = 0; - mvc->vehicleWidth = facilities.vehicle.width; - - itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ - uint16_t trajectoryA_len = 0; - - itss_trajectory_lock(); - trajectoryA_len = epv.trajectory.len; - memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t)); - itss_trajectory_unlock(); - - trajectoryA[0].latitude = lat; - trajectoryA[0].longitude = lon; - trajectoryA[0].timestamp = now; - ++trajectoryA_len; - - mvc->plannedTrajectory.list.count = trajectoryA_len - 1; - mvc->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*); - mvc->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*)); - for (int i = 0; i < trajectoryA_len - 1; ++i) { - mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); - mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude; - mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude; - mvc->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp; - } - - mvc->negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); - mvc->negotiation->present = CoordinationNegotiation_PR_commit; - mvc->negotiation->choice.commit.nonce = coordination->session.nonce; - mvc->negotiation->choice.commit.requesterId = reply->requesterId; - - tx_vcm(vcm_com); - ASN_STRUCT_FREE(asn_DEF_VCM, vcm_com); - log_warn("SENDING COMMIT"); + if (coordination->protocol != MC_PROTOCOL_REQ_REP) { + commit(); } - return rv; } @@ -708,8 +778,8 @@ static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) { neighbour->intersecting = true; neighbour->t_iid = now_us; - uint32_t intneigh[MC_AFF_STATIONS_N_MAX]; - intneigh[0] = neighbour->station_id; + mc_neighbour_s* intneigh[MC_AFF_STATIONS_N_MAX]; + intneigh[0] = neighbour; int n_intneigh = 1; for (int n = 0; n < coordination->neighbours_len && n_intneigh < MC_AFF_STATIONS_N_MAX; ++n) { if (coordination->neighbours[n].station_id == neighbour->station_id) { @@ -726,7 +796,7 @@ static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) { )) { coordination->neighbours[n].intersecting = true; coordination->neighbours[n].t_iid = now_us; - intneigh[n_intneigh] = coordination->neighbours[n].station_id; + intneigh[n_intneigh] = &coordination->neighbours[n]; ++n_intneigh; } } @@ -740,14 +810,14 @@ static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) { pt->affectingStations.list.array = malloc(n_intneigh * sizeof(void*)); for (int i = 0; i < n_intneigh; ++i) { pt->affectingStations.list.array[i] = malloc(sizeof(long long)); - *pt->affectingStations.list.array[i] = intneigh[i]; + *pt->affectingStations.list.array[i] = intneigh[i]->station_id; #ifndef NDEBUG - wr += sprintf(buffer + wr, " %d", intneigh[i]); + wr += sprintf(buffer + wr, " %d", intneigh[i]->station_id); #endif } log_debug("[vc] this VCM.request affects %d station(s):%s", n_intneigh, buffer); - coordination->session.req = vcm_req; + coordination->session.own_req = vcm_req; coordination->session.ts = now; coordination->session.n_affs_trjs = 1; coordination->session.n_affs_neighs[0] = n_intneigh; @@ -828,26 +898,20 @@ static int vcm_check_handle_commit(VCM_t* vcm, mc_neighbour_s* neighbour) { return 0; } -static mc_neighbour_s* get_neighbour(VCM_t* vcm) { +static bool in_maneuver() { coordination_t* coordination = &facilities.coordination; + uint64_t now = itss_time_get(); - mc_neighbour_s* neighbour = NULL; - - // Find neighbours - for (int i = 0; i < coordination->neighbours_len; ++i) { - if (coordination->neighbours[i].station_id == vcm->header.stationID) { - return &coordination->neighbours[i]; - } + if (now > coordination->session.ts + MC_RESOLUTION_TIMEOUT || + now > coordination->session.t_init + MC_RESOLUTION_TIMEOUT) { + session_cleanup(); + return false; + } + if (!coordination->session.own_req && !coordination->session.requester) { + return false; } - // Add it if not found - if (coordination->neighbours_len < MC_MAX_NEIGHBOURS) { - coordination->neighbours[coordination->neighbours_len].station_id = vcm->header.stationID; - ++coordination->neighbours_len; - return &coordination->neighbours[coordination->neighbours_len-1]; - } - - return NULL; + return true; } int vcm_check(VCM_t* vcm) { @@ -859,7 +923,7 @@ int vcm_check(VCM_t* vcm) { pthread_mutex_lock(&coordination->lock); - mc_neighbour_s* neighbour = get_neighbour(vcm); + mc_neighbour_s* neighbour = get_neighbour(vcm->header.stationID); switch (vcm->vcm.maneuverContainer.present) { case ManeuverContainer_PR_vehicle: @@ -883,15 +947,12 @@ int vcm_check(VCM_t* vcm) { break; } } else { - if ( - (coordination->session.req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) || /* issued request */ - now < neighbour->t_proposal + MC_RESOLUTION_TIMEOUT /* issued reply */ - ) { /* in maneuver */ + if (in_maneuver()) { log_debug("[vc] ignoring VCM from %d - currently in maneuver", vcm->header.stationID); - } else { /* not in maneuver OR timeout reached */ - if (coordination->session.req) { /* clear previous request if exists */ - ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req); - coordination->session.req = NULL; + } else { + if (coordination->session.own_req) { /* clear previous request if exists */ + ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.own_req); + coordination->session.own_req = NULL; coordination->session.ts = now; memset(coordination->session.affs, 0, sizeof(coordination->session.affs)); memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs)); diff --git a/src/vcm.h b/src/vcm.h index f08b115..65774dd 100644 --- a/src/vcm.h +++ b/src/vcm.h @@ -37,7 +37,6 @@ typedef struct mc_neighbour { uint64_t t_iid; /* timestamp of initial intersection detection */ bool proposed; - uint64_t t_proposal; /* timestamp of last request received */ } mc_neighbour_s; typedef struct coordination { @@ -52,13 +51,15 @@ typedef struct coordination { uint64_t vcm_period_max; struct { - VCM_t* req; /* last VCM.request sent */ + VCM_t* own_req; /* last VCM.request sent */ uint64_t ts; uint32_t nonce; + uint64_t t_init; /* timestamp of request rx */ + mc_neighbour_s* requester; - uint32_t affs[MC_TRAJECTORIES_N_MAX][MC_AFF_STATIONS_N_MAX]; /* trjs over affected stations */ + mc_neighbour_s* affs[MC_TRAJECTORIES_N_MAX][MC_AFF_STATIONS_N_MAX]; /* trjs over affected stations */ uint8_t n_affs_trjs; uint8_t n_affs_neighs[MC_TRAJECTORIES_N_MAX]; } session;