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

VCM-REQREPCOM

This commit is contained in:
emanuel 2023-02-01 15:21:13 +00:00
parent 28950dea7a
commit 067d727591
2 changed files with 217 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

365
src/vcm.c
View File

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

7
src/vcm.h
View File

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