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

VCM: reject if in maneuver

This commit is contained in:
emanuel 2022-11-21 16:36:57 +00:00
parent e92d2b76bb
commit 299013b59e
1 changed files with 164 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

172
src/vcm.c
View File

@ -276,6 +276,12 @@ static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) {
int rv = 0; int rv = 0;
coordination_t* coordination = &facilities.coordination; coordination_t* coordination = &facilities.coordination;
if (!coordination->session.req) {
log_debug("[coordination] unknown reply context - session expired or another ITS-S rejected");
return 1;
}
CoordinationReply_t* reply = &vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.reply; CoordinationReply_t* reply = &vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.reply;
itss_time_lock(); itss_time_lock();
@ -283,6 +289,19 @@ static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) {
itss_time_unlock(); itss_time_unlock();
if (neighbour->intersecting) { if (neighbour->intersecting) {
if (reply->acceptedTrajectoriesIds.list.count == 0) {
log_debug("[coordination] maneuver %lld rejected by %d", reply->requesterId, vcm->header.stationID);
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 1;
}
for (int t = 0; t < reply->acceptedTrajectoriesIds.list.count; ++t) { for (int t = 0; t < reply->acceptedTrajectoriesIds.list.count; ++t) {
int tid = *reply->acceptedTrajectoriesIds.list.array[t]; int tid = *reply->acceptedTrajectoriesIds.list.array[t];
if (tid > MC_TRAJECTORIES_N_MAX - 1) { if (tid > MC_TRAJECTORIES_N_MAX - 1) {
@ -322,14 +341,13 @@ static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) {
fin, fin,
now_us-neighbour->t_iid); now_us-neighbour->t_iid);
neighbour->intersecting = false;
neighbour->proposed = false;
} else { } else {
log_info("[vc] received VCM.reply is response to another ITS-S VCM.request"); log_info("[vc] received VCM.reply is response to another ITS-S VCM.request");
} }
neighbour->intersecting = false;
neighbour->proposed = false;
return rv; return rv;
} }
@ -364,7 +382,6 @@ static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) {
memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t)); memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
itss_trajectory_unlock(); itss_trajectory_unlock();
trajectoryA[0].latitude = lat; trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon; trajectoryA[0].longitude = lon;
trajectoryA[0].timestamp = now; trajectoryA[0].timestamp = now;
@ -632,6 +649,143 @@ static void intersection_check(VCM_t* vcm, mc_neighbour_s* neighbour) {
} }
} }
static void vcm_reject(VCM_t* vcm, mc_neighbour_s* neighbour) {
coordination_t* coordination = &facilities.coordination;
pthread_mutex_lock(&facilities.id.lock);
uint32_t my_station_id = 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();
uint64_t now = itss_time_get();
TransportRequest_t* tr = NULL;
FacilitiesIndication_t* fi = NULL;
VCM_t* vcm_rep = calloc(1, sizeof(VCM_t));
int rv = 0;
const uint16_t buf_len = 2048;
uint8_t buf1[buf_len];
vcm_rep->header.messageID = 43;
vcm_rep->header.protocolVersion = 1;
vcm_rep->header.stationID = my_station_id;
vcm_rep->vcm.currentPosition.latitude = lat;
vcm_rep->vcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&vcm_rep->vcm.currentPosition.timestamp, now);
if (coordination->chain.id) {
vcm_rep->vcm.chain = calloc(1, sizeof(ChainInformation_t));
vcm_rep->vcm.chain->id = coordination->chain.id;
vcm_rep->vcm.chain->link.buf = malloc(32);
vcm_rep->vcm.chain->link.size = 32;
memcpy(vcm_rep->vcm.chain->link.buf, coordination->chain.link, 32);
vcm_rep->vcm.chain->area = calloc(1, sizeof(OCTET_STRING_t));
vcm_rep->vcm.chain->area->buf = malloc(4);
vcm_rep->vcm.chain->area->size = 4;
}
vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle;
// Vehicle DimensionsDimensions
mvc_rep->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc_rep->vehicleWidth = facilities.vehicle.width;
mvc_rep->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
mvc_rep->negotiation->present = CoordinationNegotiation_PR_reply;
mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 0;
mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = 0;
mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = NULL;
mvc_rep->negotiation->choice.reply.requesterId = vcm->header.stationID;
mvc_rep->negotiation->choice.reply.nonce = vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.request.nonce;
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm_rep, buf1, buf_len);
if (enc.encoded == -1) {
log_error("[vc] VCM.reply encode failure (%s)", enc.failed_type->name);
rv = 1;
goto cleanup;
}
ssize_t vcm_rep_len = (enc.encoded + 7) / 8;
tr = calloc(1, sizeof(TransportRequest_t));
tr->present = TransportRequest_PR_packet;
tr->choice.packet.present = TransportPacketRequest_PR_btp;
BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;
bpr->btpType = BTPType_btpB;
bpr->id = itss_id(buf1, vcm_rep_len);
bpr->gn.destinationAddress.buf = malloc(6);
for (int i = 0; i < 6; ++i) {
bpr->gn.destinationAddress.buf[i] = 0xff;
}
bpr->gn.destinationAddress.size = 6;
bpr->gn.packetTransportType = PacketTransportType_shb;
bpr->destinationPort = 2043;
bpr->gn.trafficClass = 2;
bpr->data.buf = malloc(vcm_rep_len);
memcpy(bpr->data.buf, buf1, vcm_rep_len);
bpr->data.size = vcm_rep_len;
buf1[0] = 4;
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf1+1, buf_len-1);
if (enc.encoded == -1) {
log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
rv = 1;
goto cleanup;
}
itss_queue_send(facilities.tx_queue, buf1, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp");
fi = calloc(1, sizeof(FacilitiesIndication_t));
fi->present = FacilitiesIndication_PR_message;
fi->choice.message.id = bpr->id;
fi->choice.message.itsMessageType = 2043;
fi->choice.message.data.size = bpr->data.size;
fi->choice.message.data.buf = malloc(bpr->data.size);
memcpy(fi->choice.message.data.buf, bpr->data.buf, bpr->data.size);
buf1[0] = 4;
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf1+1, buf_len-1);
if (enc.encoded == -1) {
log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
rv = 1;
goto cleanup;
}
itss_queue_send(facilities.tx_queue, buf1, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");
coordination->t_last_send_vcm = now;
if (facilities.logging.recorder) {
uint16_t buffer_len = 2048;
uint8_t buffer[buffer_len];
int e = itss_management_record_packet_sdu(
buffer,
buffer_len,
bpr->data.buf,
bpr->data.size,
bpr->id,
itss_time_get(),
ITSS_FACILITIES,
true);
if (e != -1) {
itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, bpr->id, "MReq.packet.set");
}
}
cleanup:
ASN_STRUCT_FREE(asn_DEF_VCM, vcm_rep);
ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);
}
int vcm_check(VCM_t* vcm) { int vcm_check(VCM_t* vcm) {
coordination_t* coordination = &facilities.coordination; coordination_t* coordination = &facilities.coordination;
@ -673,7 +827,11 @@ int vcm_check(VCM_t* vcm) {
if (mvc->negotiation) { if (mvc->negotiation) {
switch (mvc->negotiation->present) { switch (mvc->negotiation->present) {
case CoordinationNegotiation_PR_request: case CoordinationNegotiation_PR_request:
vcm_check_handle_request(vcm, neighbour); if (coordination->session.req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) { /* in maneuver */
vcm_reject(vcm, neighbour);
} else {
vcm_check_handle_request(vcm, neighbour);
}
break; break;
case CoordinationNegotiation_PR_reply: case CoordinationNegotiation_PR_reply:
@ -688,9 +846,7 @@ int vcm_check(VCM_t* vcm) {
(coordination->session.req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) || /* issued request */ (coordination->session.req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) || /* issued request */
now < neighbour->t_proposal + MC_RESOLUTION_TIMEOUT /* issued reply */ now < neighbour->t_proposal + MC_RESOLUTION_TIMEOUT /* issued reply */
) { /* in maneuver */ ) { /* in maneuver */
log_debug("[coordination] ignoring VCM - currently in maneuver"); log_debug("[coordination] ignoring VCM from %d - currently in maneuver", vcm->header.stationID);
// TODO dont ignore-> reject
} else { /* not in maneuver OR timeout reached */ } else { /* not in maneuver OR timeout reached */
if (coordination->session.req) { /* clear previous request if exists */ if (coordination->session.req) { /* clear previous request if exists */
ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req); ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req);