722 lines
28 KiB
C
722 lines
28 KiB
C
#include "pcm.h"
|
|
#include "facilities.h"
|
|
|
|
#include <it2s-tender/epv.h>
|
|
#include <it2s-tender/space.h>
|
|
#include <it2s-tender/trajectory.h>
|
|
#include <it2s-tender/geodesy.h>
|
|
#include <it2s-tender/recorder.h>
|
|
#include <itss-transport/TransportRequest.h>
|
|
#include <itss-facilities/FacilitiesIndication.h>
|
|
#include <pcm/PCM.h>
|
|
|
|
static int are_vehicles_intersecting(
|
|
it2s_tender_st_s* tA, int tA_len, uint16_t vA_length, uint16_t vA_width,
|
|
it2s_tender_st_s* tB, int tB_len, uint16_t vB_length, uint16_t vB_width,
|
|
int* index
|
|
) {
|
|
|
|
// TODO check first time intersection, then the spacial trajectory
|
|
double A1[2], A2[2], B1[2], B2[2];
|
|
uint64_t tsA, tsB;
|
|
for (int a = 0; a < tA_len; ++a) {
|
|
// for (int a = 0; a < tA_len-1; ++a) {
|
|
// A1[0] = tA[a].latitude;
|
|
// A1[1] = tA[a].longitude;
|
|
// A2[0] = tA[a+1].latitude;
|
|
// A2[1] = tA[a+1].longitude;
|
|
|
|
|
|
for (int b = 0; b < tB_len; ++b) {
|
|
// for (int b = 0; b < tB_len-1; ++b) {
|
|
// B1[0] = tB[b].latitude;
|
|
// B1[1] = tB[b].longitude;
|
|
// B2[0] = tB[b+1].latitude;
|
|
// B2[1] = tB[b+1].longitude;
|
|
|
|
// if (it2s_tender_do_segments_intersect(A1, A2, B1, B2)) {
|
|
if (it2s_tender_is_inside_circle(tA[a].latitude, tA[a].longitude, tB[b].latitude, tB[b].longitude, 8, DCM_HAVERSINE)) {
|
|
if (tA[a].timestamp < tB[b].timestamp + 2000 &&
|
|
tA[a].timestamp > tB[b].timestamp - 2000) {
|
|
*index = a;
|
|
return 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_neighbour_s* neighbour) {
|
|
int rv = 0;
|
|
|
|
coordination_s* coordination = &facilities->coordination;
|
|
|
|
CoordinationRequest_t* request = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.request;
|
|
|
|
PCM_t* pcm_rep = NULL;
|
|
TransportRequest_t* tr = NULL;
|
|
FacilitiesIndication_t* fi = NULL;
|
|
|
|
const ssize_t buf_len = 512;
|
|
uint8_t buf[buf_len];
|
|
|
|
it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
|
|
ssize_t trajectoryA_len = 0;
|
|
|
|
uint64_t now = it2s_tender_get_clock(&facilities->epv);
|
|
|
|
neighbour->proposed = true;
|
|
neighbour->t_proposal = now;
|
|
|
|
int32_t lat, lon;
|
|
it2s_tender_lock_space(&facilities->epv);
|
|
it2s_tender_get_space(&facilities->epv);
|
|
lat = facilities->epv.space.latitude;
|
|
lon = facilities->epv.space.longitude;
|
|
it2s_tender_unlock_space(&facilities->epv);
|
|
|
|
it2s_tender_lock_trajectory(&facilities->epv);
|
|
trajectoryA_len = facilities->epv.trajectory.len;
|
|
memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s));
|
|
it2s_tender_unlock_trajectory(&facilities->epv);
|
|
|
|
trajectoryA[0].latitude = lat;
|
|
trajectoryA[0].longitude = lon;
|
|
trajectoryA[0].timestamp = now;
|
|
++trajectoryA_len;
|
|
|
|
pcm_rep = calloc(1, sizeof(PCM_t));
|
|
|
|
pcm_rep->header.messageID = 43;
|
|
pcm_rep->header.protocolVersion = 1;
|
|
pcm_rep->header.stationID = 1;
|
|
pthread_mutex_lock(&facilities->id.lock);
|
|
pcm_rep->header.stationID = facilities->id.station_id;
|
|
pthread_mutex_unlock(&facilities->id.lock);
|
|
|
|
pcm_rep->pcm.currentPosition.latitude = lat;
|
|
pcm_rep->pcm.currentPosition.longitude = lon;
|
|
asn_ulong2INTEGER(&pcm_rep->pcm.currentPosition.timestamp, now);
|
|
|
|
if (coordination->chain.id) {
|
|
pcm_rep->pcm.chain = calloc(1, sizeof(ChainInformation_t));
|
|
pcm_rep->pcm.chain->id = coordination->chain.id;
|
|
pcm_rep->pcm.chain->link.buf = malloc(8);
|
|
pcm_rep->pcm.chain->link.size = 8;
|
|
memcpy(pcm_rep->pcm.chain->link.buf, coordination->chain.link, 8);
|
|
pcm_rep->pcm.chain->area.present = ChainGeoRegion_PR_circle;
|
|
}
|
|
|
|
pcm_rep->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
|
|
ManeuverVehicleContainer_t* mvc = &pcm_rep->pcm.maneuverContainer.choice.vehicle;
|
|
|
|
// Vehicle DimensionsDimensions
|
|
mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
|
|
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
|
|
mvc->vehicleWidth = facilities->vehicle.width;
|
|
|
|
// Accepted trajectory
|
|
if (!request->desiredTrajectories.list.count) {
|
|
syslog_debug("[facilities] [pc] received PCM request has no desired trajectories");
|
|
rv = 1;
|
|
goto cleanup;
|
|
}
|
|
ProposedTrajectory_t* pt = request->desiredTrajectories.list.array[0];
|
|
|
|
mvc->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
|
|
mvc->negotiation->present = CoordinationNegotiation_PR_reply;
|
|
mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 1;
|
|
mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = sizeof(void*);
|
|
mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = malloc(1*sizeof(void*));
|
|
mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = malloc(sizeof(long));
|
|
*mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = pt->id;
|
|
mvc->negotiation->choice.reply.requesterId = pcm->header.stationID;
|
|
|
|
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_PCM, NULL, pcm_rep, buf, buf_len);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] [pc] PCM.reply encode failure (%s)", enc.failed_type->name);
|
|
rv = 1;
|
|
goto cleanup;
|
|
}
|
|
ssize_t pcm_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 = rand() + 1;
|
|
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(pcm_rep_len);
|
|
memcpy(bpr->data.buf, buf, pcm_rep_len);
|
|
bpr->data.size = pcm_rep_len;
|
|
buf[0] = 4;
|
|
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] [pc] TR PCM.reply encode failure (%s)", enc.failed_type->name);
|
|
rv = 1;
|
|
goto cleanup;
|
|
}
|
|
|
|
it2s_tender_queue_send(facilities->tx_queue, buf, 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);
|
|
buf[0] = 4;
|
|
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, buf_len-1);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] [pc] TR PCM.reply encode failure (%s)", enc.failed_type->name);
|
|
rv = 1;
|
|
goto cleanup;
|
|
}
|
|
|
|
it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");
|
|
|
|
if (facilities->logging.recorder) {
|
|
uint16_t buffer_len = 2048;
|
|
uint8_t buffer[buffer_len];
|
|
int e = it2s_tender_management_record_packet_sdu(
|
|
buffer,
|
|
buffer_len,
|
|
bpr->data.buf,
|
|
bpr->data.size,
|
|
bpr->id,
|
|
it2s_tender_get_clock(&facilities->epv),
|
|
ITSS_FACILITIES,
|
|
true);
|
|
if (e != -1) {
|
|
it2s_tender_queue_send(facilities->tx_queue, buffer, e, ITSS_MANAGEMENT, bpr->id, "MReq.packet.set");
|
|
}
|
|
}
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_PCM, pcm_rep);
|
|
ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
|
|
ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);
|
|
return rv;
|
|
}
|
|
|
|
static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm, mc_neighbour_s* neighbour) {
|
|
int rv = 0;
|
|
|
|
CoordinationReply_t* reply = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.reply;
|
|
|
|
pthread_mutex_lock(&facilities->epv.time.lock);
|
|
uint64_t now_us = it2s_tender_get_now(TIME_MICROSECONDS);
|
|
pthread_mutex_unlock(&facilities->epv.time.lock);
|
|
|
|
if (neighbour->intersecting) {
|
|
syslog_info("[facilities] [pc] received PCM.reply with %d accepted trajectories | took %ld us", reply->acceptedTrajectoriesIds.list.count, now_us-neighbour->t_iid);
|
|
} else {
|
|
syslog_info("[facilities] [pc] received PCM.reply is response to another ITS-S PCM.request");
|
|
}
|
|
|
|
|
|
neighbour->intersecting = false;
|
|
neighbour->proposed = false;
|
|
|
|
return rv;
|
|
}
|
|
|
|
static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm, mc_neighbour_s* neighbour) {
|
|
|
|
int rv = 0;
|
|
|
|
coordination_s* coordination = &facilities->coordination;
|
|
|
|
PCM_t* pcm_req = NULL;
|
|
TransportRequest_t* tr = NULL;
|
|
FacilitiesIndication_t* fi = NULL;
|
|
|
|
const ssize_t buf_len = 1024;
|
|
uint8_t buf[buf_len];
|
|
|
|
it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
|
|
uint16_t trajectoryA_len = 0;
|
|
|
|
uint64_t now = it2s_tender_get_clock(&facilities->epv);
|
|
|
|
if (now < neighbour->t_iid/1000 + MC_RESOLUTION_TIMEOUT) {
|
|
rv = 0;
|
|
goto cleanup;
|
|
}
|
|
|
|
neighbour->intersecting = true;
|
|
|
|
pthread_mutex_lock(&facilities->epv.time.lock);
|
|
uint64_t now_us = it2s_tender_get_now(TIME_MICROSECONDS) ;
|
|
pthread_mutex_unlock(&facilities->epv.time.lock);
|
|
neighbour->t_iid = now_us;
|
|
|
|
if (now < neighbour->t_proposal + MC_RESOLUTION_TIMEOUT) {
|
|
rv = 0;
|
|
goto cleanup;
|
|
}
|
|
|
|
int32_t lat, lon;
|
|
it2s_tender_lock_space(&facilities->epv);
|
|
it2s_tender_get_space(&facilities->epv);
|
|
lat = facilities->epv.space.latitude;
|
|
lon = facilities->epv.space.longitude;
|
|
it2s_tender_unlock_space(&facilities->epv);
|
|
|
|
it2s_tender_lock_trajectory(&facilities->epv);
|
|
trajectoryA_len = facilities->epv.trajectory.len;
|
|
memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s));
|
|
it2s_tender_unlock_trajectory(&facilities->epv);
|
|
|
|
|
|
trajectoryA[0].latitude = lat;
|
|
trajectoryA[0].longitude = lon;
|
|
trajectoryA[0].timestamp = now;
|
|
++trajectoryA_len;
|
|
|
|
// Initiate conflict resolution
|
|
pcm_req = calloc(1, sizeof(PCM_t));
|
|
|
|
pcm_req->header.messageID = 43;
|
|
pcm_req->header.protocolVersion = 1;
|
|
pcm_req->header.stationID = 1;
|
|
pthread_mutex_lock(&facilities->id.lock);
|
|
pcm_req->header.stationID = facilities->id.station_id;
|
|
pthread_mutex_unlock(&facilities->id.lock);
|
|
|
|
pcm_req->pcm.currentPosition.latitude = lat;
|
|
pcm_req->pcm.currentPosition.longitude = lon;
|
|
asn_ulong2INTEGER(&pcm_req->pcm.currentPosition.timestamp, now);
|
|
|
|
if (coordination->chain.id) {
|
|
pcm_req->pcm.chain = calloc(1, sizeof(ChainInformation_t));
|
|
pcm_req->pcm.chain->id = coordination->chain.id;
|
|
pcm_req->pcm.chain->link.buf = malloc(8);
|
|
pcm_req->pcm.chain->link.size = 8;
|
|
memcpy(pcm_req->pcm.chain->link.buf, coordination->chain.link, 8);
|
|
pcm_req->pcm.chain->area.present = ChainGeoRegion_PR_circle;
|
|
}
|
|
|
|
pcm_req->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
|
|
ManeuverVehicleContainer_t* mvc = &pcm_req->pcm.maneuverContainer.choice.vehicle;
|
|
|
|
// Vehicle Dimensions
|
|
mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
|
|
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
|
|
mvc->vehicleWidth = facilities->vehicle.width;
|
|
|
|
// Planned trajectory
|
|
mvc->plannedTrajectory.list.count = --trajectoryA_len;
|
|
mvc->plannedTrajectory.list.size = trajectoryA_len * sizeof(void*);
|
|
mvc->plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*));
|
|
for (int i = 0; i < trajectoryA_len; ++i) {
|
|
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
|
|
mvc->plannedTrajectory.list.array[i]->latitude = trajectoryA[i+1].latitude;
|
|
mvc->plannedTrajectory.list.array[i]->longitude = trajectoryA[i+1].longitude;
|
|
asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[i]->timestamp, trajectoryA[i+1].timestamp);
|
|
}
|
|
|
|
// Desired trajectory
|
|
mvc->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
|
|
mvc->negotiation->present = CoordinationNegotiation_PR_request;
|
|
mvc->negotiation->choice.request.desiredTrajectories.list.count = 1;
|
|
mvc->negotiation->choice.request.desiredTrajectories.list.size = sizeof(void*);
|
|
mvc->negotiation->choice.request.desiredTrajectories.list.array = malloc(1*sizeof(void*));
|
|
mvc->negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t));
|
|
ProposedTrajectory_t* pt = mvc->negotiation->choice.request.desiredTrajectories.list.array[0];
|
|
mvc->negotiation->choice.request.requesterId = pcm->header.stationID;
|
|
|
|
pt->trajectory.list.count = trajectoryA_len;
|
|
pt->trajectory.list.size = sizeof(void*) * trajectoryA_len;
|
|
pt->trajectory.list.array = malloc(sizeof(void*) * trajectoryA_len);
|
|
for (int i = 0; i < trajectoryA_len; ++i) {
|
|
pt->trajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
|
|
pt->trajectory.list.array[i]->latitude = trajectoryA[i+1].latitude;
|
|
pt->trajectory.list.array[i]->longitude = trajectoryA[i+1].longitude;
|
|
asn_ulong2INTEGER(&pt->trajectory.list.array[i]->timestamp, trajectoryA[i+1].timestamp);
|
|
}
|
|
|
|
pt->offer = 5;
|
|
pt->priority = 1;
|
|
pt->id = 0;
|
|
|
|
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_PCM, NULL, pcm_req, buf, buf_len);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] [pc] PCM.request encode failure (%s)", enc.failed_type->name);
|
|
rv = 1;
|
|
goto cleanup;
|
|
}
|
|
ssize_t pcm_req_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->id = rand() + 1;
|
|
bpr->btpType = BTPType_btpB;
|
|
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(pcm_req_len);
|
|
memcpy(bpr->data.buf, buf, pcm_req_len);
|
|
bpr->data.size = pcm_req_len;
|
|
buf[0] = 4;
|
|
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] [pc] TR PCM.request encode failure (%s)", enc.failed_type->name);
|
|
rv = 1;
|
|
goto cleanup;
|
|
}
|
|
|
|
it2s_tender_queue_send(facilities->tx_queue, buf, 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);
|
|
buf[0] = 4;
|
|
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, buf_len-1);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] [pc] TR PCM.reply encode failure (%s)", enc.failed_type->name);
|
|
rv = 1;
|
|
goto cleanup;
|
|
}
|
|
|
|
it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");
|
|
|
|
if (facilities->logging.recorder) {
|
|
uint16_t buffer_len = 2048;
|
|
uint8_t buffer[buffer_len];
|
|
int e = it2s_tender_management_record_packet_sdu(
|
|
buffer,
|
|
buffer_len,
|
|
bpr->data.buf,
|
|
bpr->data.size,
|
|
bpr->id,
|
|
it2s_tender_get_clock(&facilities->epv),
|
|
ITSS_FACILITIES,
|
|
true);
|
|
if (e != -1) {
|
|
it2s_tender_queue_send(facilities->tx_queue, buffer, e, ITSS_MANAGEMENT, bpr->id, "MReq.packet.set");
|
|
}
|
|
}
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_PCM, pcm_req);
|
|
ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
|
|
ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);
|
|
return rv;
|
|
}
|
|
|
|
int pcm_check(void* fc, PCM_t* pcm) {
|
|
facilities_t* facilities = (facilities_t*) fc;
|
|
coordination_s* coordination = &facilities->coordination;
|
|
|
|
int rv = 0;
|
|
|
|
it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
|
|
it2s_tender_st_s trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */
|
|
uint16_t trajectoryA_len = 0;
|
|
uint16_t trajectoryB_len = 0;
|
|
|
|
uint64_t now = it2s_tender_get_clock(&facilities->epv);
|
|
|
|
int32_t lat, lon;
|
|
it2s_tender_lock_space(&facilities->epv);
|
|
it2s_tender_get_space(&facilities->epv);
|
|
lat = facilities->epv.space.latitude;
|
|
lon = facilities->epv.space.longitude;
|
|
it2s_tender_unlock_space(&facilities->epv);
|
|
|
|
pthread_mutex_lock(&coordination->lock);
|
|
|
|
// Find neighbours
|
|
int ni = -1;
|
|
for (int i = 0; i < coordination->neighbours_len; ++i) {
|
|
if (coordination->neighbours->station_id == pcm->header.stationID) {
|
|
ni = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Add it if not found
|
|
if (ni == -1 && coordination->neighbours_len + 1 < MC_MAX_NEIGHBOURS) {
|
|
coordination->neighbours[coordination->neighbours_len].station_id = pcm->header.stationID;
|
|
ni = coordination->neighbours_len;
|
|
++coordination->neighbours_len;
|
|
}
|
|
|
|
switch (pcm->pcm.maneuverContainer.present) {
|
|
case ManeuverContainer_PR_vehicle:
|
|
;
|
|
ManeuverVehicleContainer_t* mvc = &pcm->pcm.maneuverContainer.choice.vehicle;
|
|
if (mvc->negotiation) {
|
|
switch (mvc->negotiation->present) {
|
|
case CoordinationNegotiation_PR_request:
|
|
pcm_check_handle_request(facilities, pcm, &coordination->neighbours[ni]);
|
|
break;
|
|
case CoordinationNegotiation_PR_reply:
|
|
pcm_check_handle_reply(facilities, pcm, &coordination->neighbours[ni]);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
} else {
|
|
|
|
it2s_tender_lock_trajectory(&facilities->epv);
|
|
trajectoryA_len = facilities->epv.trajectory.len;
|
|
memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s));
|
|
it2s_tender_unlock_trajectory(&facilities->epv);
|
|
|
|
trajectoryA[0].latitude = lat;
|
|
trajectoryA[0].longitude = lon;
|
|
trajectoryA[0].timestamp = now;
|
|
++trajectoryA_len;
|
|
|
|
trajectoryB[0].latitude = pcm->pcm.currentPosition.latitude;
|
|
trajectoryB[0].longitude = pcm->pcm.currentPosition.longitude;
|
|
asn_INTEGER2ulong(&pcm->pcm.currentPosition.timestamp, &trajectoryB[0].timestamp);
|
|
++trajectoryB_len;
|
|
for (int i = 0; i < mvc->plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) {
|
|
trajectoryB[i+1].latitude = mvc->plannedTrajectory.list.array[i]->latitude;
|
|
trajectoryB[i+1].longitude = mvc->plannedTrajectory.list.array[i]->longitude;
|
|
asn_INTEGER2ulong(&mvc->plannedTrajectory.list.array[i]->timestamp, &trajectoryB[i+1].timestamp);
|
|
++trajectoryB_len;
|
|
}
|
|
|
|
bool intersecting = false;
|
|
int index = -1;
|
|
|
|
if (trajectoryA_len > 1 && trajectoryB_len > 1) {
|
|
intersecting = are_vehicles_intersecting(
|
|
trajectoryA, trajectoryA_len, facilities->vehicle.length, facilities->vehicle.width,
|
|
trajectoryB, trajectoryB_len, mvc->vehicleLength.vehicleLengthValue, mvc->vehicleWidth,
|
|
&index);
|
|
}
|
|
|
|
if (intersecting) {
|
|
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_check_intersection_detected(facilities, pcm, &coordination->neighbours[ni]);
|
|
}
|
|
}
|
|
break;
|
|
case ManeuverContainer_PR_rsu:
|
|
break;
|
|
default:
|
|
syslog_debug("[facilities] [pc] received PCM contains unrecognized ManeuverContainer type");
|
|
rv = 1;
|
|
}
|
|
|
|
pthread_mutex_unlock(&coordination->lock);
|
|
|
|
return rv;
|
|
}
|
|
|
|
static int mk_pcm(facilities_t* facilities, uint8_t* pcm_uper, uint16_t* pcm_uper_len) {
|
|
int rv = 0;
|
|
|
|
coordination_s* coordination = &facilities->coordination;
|
|
|
|
it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN];
|
|
|
|
PCM_t* pcm = calloc(1, sizeof(PCM_t));
|
|
|
|
pcm->header.messageID = 43;
|
|
pcm->header.protocolVersion = 1;
|
|
uint64_t now = it2s_tender_get_clock(&facilities->epv);
|
|
pthread_mutex_lock(&facilities->id.lock);
|
|
pcm->header.stationID = facilities->id.station_id;
|
|
pthread_mutex_unlock(&facilities->id.lock);
|
|
|
|
int32_t lat, lon;
|
|
uint16_t trajectory_len = 0;
|
|
it2s_tender_lock_space(&facilities->epv);
|
|
it2s_tender_get_space(&facilities->epv);
|
|
lat = facilities->epv.space.latitude;
|
|
lon = facilities->epv.space.longitude;
|
|
it2s_tender_unlock_space(&facilities->epv);
|
|
|
|
it2s_tender_lock_trajectory(&facilities->epv);
|
|
trajectory_len = facilities->epv.trajectory.len;
|
|
memcpy(trajectory, facilities->epv.trajectory.path, trajectory_len * sizeof(it2s_tender_st_s));
|
|
it2s_tender_unlock_trajectory(&facilities->epv);
|
|
|
|
pcm->pcm.currentPosition.latitude = lat;
|
|
pcm->pcm.currentPosition.longitude = lon;
|
|
asn_ulong2INTEGER(&pcm->pcm.currentPosition.timestamp, now);
|
|
|
|
if (coordination->chain.id) {
|
|
pcm->pcm.chain = calloc(1, sizeof(ChainInformation_t));
|
|
pcm->pcm.chain->id = coordination->chain.id;
|
|
pcm->pcm.chain->link.buf = malloc(8);
|
|
pcm->pcm.chain->link.size = 8;
|
|
memcpy(pcm->pcm.chain->link.buf, coordination->chain.link, 8);
|
|
pcm->pcm.chain->area.present = ChainGeoRegion_PR_circle;
|
|
}
|
|
|
|
if (facilities->station_type == StationType_roadSideUnit) {
|
|
pcm->pcm.maneuverContainer.present = ManeuverContainer_PR_rsu;
|
|
ManeuverRSUContainer_t* mrc = &pcm->pcm.maneuverContainer.choice.rsu;
|
|
mrc->recommendedTrajectories = NULL; // TODO
|
|
} else {
|
|
pcm->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
|
|
ManeuverVehicleContainer_t* mvc = &pcm->pcm.maneuverContainer.choice.vehicle;
|
|
// Vehicle Dimensions
|
|
mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
|
|
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
|
|
mvc->vehicleWidth = facilities->vehicle.width;
|
|
|
|
mvc->plannedTrajectory.list.count = trajectory_len;
|
|
mvc->plannedTrajectory.list.size = trajectory_len * sizeof(void*);
|
|
mvc->plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*));
|
|
for (int i = 0; i < trajectory_len; ++i) {
|
|
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
|
|
mvc->plannedTrajectory.list.array[i]->latitude = trajectory[i].latitude;
|
|
mvc->plannedTrajectory.list.array[i]->longitude = trajectory[i].longitude;
|
|
asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[i]->timestamp, trajectory[i].timestamp);
|
|
}
|
|
}
|
|
|
|
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_PCM, NULL, pcm, pcm_uper, 512);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] [pc] failed encoding PCM (%s)", enc.failed_type->name);
|
|
rv = 1;
|
|
goto cleanup;
|
|
}
|
|
*pcm_uper_len = (enc.encoded + 7) / 8;
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_PCM, pcm);
|
|
|
|
return rv;
|
|
}
|
|
|
|
static bool pcm_timer_check(coordination_s* coordination, uint64_t now) {
|
|
|
|
bool send = false;
|
|
|
|
if (now > coordination->t_last_send_pcm + coordination->pcm_period_max) {
|
|
send = true;
|
|
}
|
|
|
|
return send;
|
|
}
|
|
|
|
void* pc_service(void* fc) {
|
|
|
|
facilities_t* facilities = (facilities_t*) fc;
|
|
coordination_s* coordination = (coordination_s*) &facilities->coordination;
|
|
|
|
uint8_t pcm[512];
|
|
|
|
TransportRequest_t* 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->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(512);
|
|
|
|
FacilitiesIndication_t* fi = calloc(1, sizeof(FacilitiesIndication_t));
|
|
fi = calloc(1, sizeof(FacilitiesIndication_t));
|
|
fi->present = FacilitiesIndication_PR_message;
|
|
fi->choice.message.itsMessageType = 2043;
|
|
fi->choice.message.data.buf = malloc(512);
|
|
|
|
uint8_t buf[1024];
|
|
buf[0] = 4;
|
|
|
|
int rv;
|
|
|
|
while (!facilities->exit) {
|
|
|
|
uint64_t now = it2s_tender_get_clock(&facilities->epv);
|
|
|
|
pthread_mutex_lock(&coordination->lock);
|
|
if (pcm_timer_check(coordination, now)) {
|
|
rv = mk_pcm(facilities, bpr->data.buf, (uint16_t *) &bpr->data.size);
|
|
if (rv) {
|
|
continue;
|
|
}
|
|
|
|
bpr->id = rand() + 1;
|
|
|
|
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, buf+1, 1023);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] encoding TR for PCM failed");
|
|
continue;
|
|
}
|
|
it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp");
|
|
|
|
fi->choice.message.id = bpr->id;
|
|
fi->choice.message.data.size = bpr->data.size;
|
|
memcpy(fi->choice.message.data.buf, bpr->data.buf, bpr->data.size);
|
|
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, 1023);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] encoding FI for PCM failed");
|
|
continue;
|
|
}
|
|
it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");
|
|
|
|
if (facilities->logging.recorder) {
|
|
uint16_t buffer_len = 2048;
|
|
uint8_t buffer[buffer_len];
|
|
int e = it2s_tender_management_record_packet_sdu(
|
|
buffer,
|
|
buffer_len,
|
|
bpr->data.buf,
|
|
bpr->data.size,
|
|
bpr->id,
|
|
it2s_tender_get_clock(&facilities->epv),
|
|
ITSS_FACILITIES,
|
|
true);
|
|
if (e != -1) {
|
|
it2s_tender_queue_send(facilities->tx_queue, buffer, e, ITSS_MANAGEMENT, bpr->id, "MReq.packet.set");
|
|
}
|
|
}
|
|
|
|
coordination->t_last_send_pcm = now;
|
|
}
|
|
pthread_mutex_unlock(&coordination->lock);
|
|
|
|
usleep(50 * 1000);
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
void coordination_init(coordination_s* coordination) {
|
|
pthread_mutex_init(&coordination->lock, NULL);
|
|
}
|