VCM-REQREPCOM
This commit is contained in:
parent
28950dea7a
commit
067d727591
357
src/vcm.c
357
src/vcm.c
|
|
@ -46,6 +46,29 @@ static int do_paths_intersect(
|
||||||
return 0;
|
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) {
|
static void tx_vcm(VCM_t* vcm) {
|
||||||
const uint16_t buf_len = 2048;
|
const uint16_t buf_len = 2048;
|
||||||
uint8_t buf[buf_len];
|
uint8_t buf[buf_len];
|
||||||
|
|
@ -220,6 +243,110 @@ cleanup:
|
||||||
ASN_STRUCT_FREE(asn_DEF_VCM, vcm_rep);
|
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) {
|
static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
|
||||||
int rv = 0;
|
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();
|
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);
|
log_debug("[vc] rejecting VCM from %d - currently in maneuver", vcm->header.stationID);
|
||||||
vcm_reject(vcm, neighbour);
|
vcm_reject(vcm, neighbour);
|
||||||
return 1;
|
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]) {
|
if (my_station_id == *request->desiredTrajectories.list.array[i]->affectingStations.list.array[j]) {
|
||||||
is_req4me = true;
|
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;
|
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 */
|
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
|
||||||
ssize_t trajectoryA_len = 0;
|
ssize_t trajectoryA_len = 0;
|
||||||
|
|
||||||
neighbour->t_proposal = now;
|
|
||||||
neighbour->proposed = true;
|
neighbour->proposed = true;
|
||||||
neighbour->session.requested = true;
|
neighbour->session.requested = true;
|
||||||
neighbour->session.nonce = request->nonce;
|
neighbour->session.nonce = request->nonce;
|
||||||
neighbour->station_id = vcm->header.stationID;
|
neighbour->station_id = vcm->header.stationID;
|
||||||
coordination->session.requester = neighbour;
|
coordination->session.requester = neighbour;
|
||||||
|
coordination->session.t_init = now;
|
||||||
|
|
||||||
vcm_rep = calloc(1, sizeof(VCM_t));
|
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);
|
tx_vcm(vcm_rep);
|
||||||
|
|
||||||
cleanup:
|
for (int t = 0; t < mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count; ++t) {
|
||||||
ASN_STRUCT_FREE(asn_DEF_VCM, vcm_rep);
|
int tid = *mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[t];
|
||||||
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 n = 0; n < coordination->session.n_affs_neighs[tid]; ++n) {
|
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) {
|
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.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) {
|
static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) {
|
||||||
int rv = 0;
|
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);
|
uint64_t now_us = itss_ts_get(TIME_MICROSECONDS);
|
||||||
itss_time_unlock();
|
itss_time_unlock();
|
||||||
|
|
||||||
bool send_commit = false;
|
|
||||||
|
|
||||||
if (neighbour->intersecting) { /* REQUESTER */
|
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");
|
log_debug("[vc] unknown reply context - session expired or another ITS-S rejected");
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!is_maneuver_approved(reply, neighbour)) {
|
if (reply->acceptedTrajectoriesIds.list.count == 0) {
|
||||||
|
session_cleanup();
|
||||||
return 1;
|
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",
|
log_info("[vc] received VCM.reply from %d with %d accepted trajectories | took %ld us",
|
||||||
vcm->header.stationID,
|
vcm->header.stationID,
|
||||||
reply->acceptedTrajectoriesIds.list.count,
|
reply->acceptedTrajectoriesIds.list.count,
|
||||||
|
|
@ -500,92 +616,46 @@ static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) {
|
||||||
neighbour->intersecting = false;
|
neighbour->intersecting = false;
|
||||||
neighbour->proposed = false;
|
neighbour->proposed = false;
|
||||||
|
|
||||||
if (coordination->protocol == MC_PROTOCOL_REQ_REP_COM) {
|
|
||||||
send_commit = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
} else { /* REPLIER */
|
} else { /* REPLIER */
|
||||||
log_debug("[vc] received VCM.reply is response to another ITS-S VCM.request");
|
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) {
|
if (reply->nonce != coordination->session.nonce) {
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!coordination->session.requester) {
|
if (reply->acceptedTrajectoriesIds.list.count == 0) {
|
||||||
|
session_cleanup();
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!is_maneuver_approved(reply, coordination->session.requester)) {
|
for (int t = 0; t < reply->acceptedTrajectoriesIds.list.count; ++t) {
|
||||||
send_commit = true;
|
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 (send_commit) {
|
if (!is_maneuver_approved()) {
|
||||||
VCM_t* vcm_com = calloc(1, sizeof(VCM_t));
|
return 1;
|
||||||
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));
|
if (coordination->protocol == MC_PROTOCOL_REQ_REP) {
|
||||||
mvc->negotiation->present = CoordinationNegotiation_PR_commit;
|
session_cleanup();
|
||||||
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;
|
return rv;
|
||||||
}
|
}
|
||||||
|
|
@ -708,8 +778,8 @@ static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) {
|
||||||
neighbour->intersecting = true;
|
neighbour->intersecting = true;
|
||||||
neighbour->t_iid = now_us;
|
neighbour->t_iid = now_us;
|
||||||
|
|
||||||
uint32_t intneigh[MC_AFF_STATIONS_N_MAX];
|
mc_neighbour_s* intneigh[MC_AFF_STATIONS_N_MAX];
|
||||||
intneigh[0] = neighbour->station_id;
|
intneigh[0] = neighbour;
|
||||||
int n_intneigh = 1;
|
int n_intneigh = 1;
|
||||||
for (int n = 0; n < coordination->neighbours_len && n_intneigh < MC_AFF_STATIONS_N_MAX; ++n) {
|
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) {
|
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].intersecting = true;
|
||||||
coordination->neighbours[n].t_iid = now_us;
|
coordination->neighbours[n].t_iid = now_us;
|
||||||
intneigh[n_intneigh] = coordination->neighbours[n].station_id;
|
intneigh[n_intneigh] = &coordination->neighbours[n];
|
||||||
++n_intneigh;
|
++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*));
|
pt->affectingStations.list.array = malloc(n_intneigh * sizeof(void*));
|
||||||
for (int i = 0; i < n_intneigh; ++i) {
|
for (int i = 0; i < n_intneigh; ++i) {
|
||||||
pt->affectingStations.list.array[i] = malloc(sizeof(long long));
|
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
|
#ifndef NDEBUG
|
||||||
wr += sprintf(buffer + wr, " %d", intneigh[i]);
|
wr += sprintf(buffer + wr, " %d", intneigh[i]->station_id);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
log_debug("[vc] this VCM.request affects %d station(s):%s", n_intneigh, buffer);
|
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.ts = now;
|
||||||
coordination->session.n_affs_trjs = 1;
|
coordination->session.n_affs_trjs = 1;
|
||||||
coordination->session.n_affs_neighs[0] = n_intneigh;
|
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;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static mc_neighbour_s* get_neighbour(VCM_t* vcm) {
|
static bool in_maneuver() {
|
||||||
coordination_t* coordination = &facilities.coordination;
|
coordination_t* coordination = &facilities.coordination;
|
||||||
|
uint64_t now = itss_time_get();
|
||||||
|
|
||||||
mc_neighbour_s* neighbour = NULL;
|
if (now > coordination->session.ts + MC_RESOLUTION_TIMEOUT ||
|
||||||
|
now > coordination->session.t_init + MC_RESOLUTION_TIMEOUT) {
|
||||||
// Find neighbours
|
session_cleanup();
|
||||||
for (int i = 0; i < coordination->neighbours_len; ++i) {
|
return false;
|
||||||
if (coordination->neighbours[i].station_id == vcm->header.stationID) {
|
|
||||||
return &coordination->neighbours[i];
|
|
||||||
}
|
}
|
||||||
|
if (!coordination->session.own_req && !coordination->session.requester) {
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Add it if not found
|
return true;
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int vcm_check(VCM_t* vcm) {
|
int vcm_check(VCM_t* vcm) {
|
||||||
|
|
@ -859,7 +923,7 @@ int vcm_check(VCM_t* vcm) {
|
||||||
|
|
||||||
pthread_mutex_lock(&coordination->lock);
|
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) {
|
switch (vcm->vcm.maneuverContainer.present) {
|
||||||
case ManeuverContainer_PR_vehicle:
|
case ManeuverContainer_PR_vehicle:
|
||||||
|
|
@ -883,15 +947,12 @@ int vcm_check(VCM_t* vcm) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
if (
|
if (in_maneuver()) {
|
||||||
(coordination->session.req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) || /* issued request */
|
|
||||||
now < neighbour->t_proposal + MC_RESOLUTION_TIMEOUT /* issued reply */
|
|
||||||
) { /* in maneuver */
|
|
||||||
log_debug("[vc] ignoring VCM from %d - currently in maneuver", vcm->header.stationID);
|
log_debug("[vc] ignoring VCM from %d - currently in maneuver", vcm->header.stationID);
|
||||||
} else { /* not in maneuver OR timeout reached */
|
} else {
|
||||||
if (coordination->session.req) { /* clear previous request if exists */
|
if (coordination->session.own_req) { /* clear previous request if exists */
|
||||||
ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req);
|
ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.own_req);
|
||||||
coordination->session.req = NULL;
|
coordination->session.own_req = NULL;
|
||||||
coordination->session.ts = now;
|
coordination->session.ts = now;
|
||||||
memset(coordination->session.affs, 0, sizeof(coordination->session.affs));
|
memset(coordination->session.affs, 0, sizeof(coordination->session.affs));
|
||||||
memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs));
|
memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs));
|
||||||
|
|
|
||||||
|
|
@ -37,7 +37,6 @@ typedef struct mc_neighbour {
|
||||||
uint64_t t_iid; /* timestamp of initial intersection detection */
|
uint64_t t_iid; /* timestamp of initial intersection detection */
|
||||||
|
|
||||||
bool proposed;
|
bool proposed;
|
||||||
uint64_t t_proposal; /* timestamp of last request received */
|
|
||||||
} mc_neighbour_s;
|
} mc_neighbour_s;
|
||||||
|
|
||||||
typedef struct coordination {
|
typedef struct coordination {
|
||||||
|
|
@ -52,13 +51,15 @@ typedef struct coordination {
|
||||||
uint64_t vcm_period_max;
|
uint64_t vcm_period_max;
|
||||||
|
|
||||||
struct {
|
struct {
|
||||||
VCM_t* req; /* last VCM.request sent */
|
VCM_t* own_req; /* last VCM.request sent */
|
||||||
uint64_t ts;
|
uint64_t ts;
|
||||||
uint32_t nonce;
|
uint32_t nonce;
|
||||||
|
|
||||||
|
uint64_t t_init; /* timestamp of request rx */
|
||||||
|
|
||||||
mc_neighbour_s* requester;
|
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_trjs;
|
||||||
uint8_t n_affs_neighs[MC_TRAJECTORIES_N_MAX];
|
uint8_t n_affs_neighs[MC_TRAJECTORIES_N_MAX];
|
||||||
} session;
|
} session;
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue