DCM comm efforts
This commit is contained in:
emanuel
parent
158da9f831
commit
eea1f89a55
247
src/dcm.c
247
src/dcm.c
|
|
@ -49,21 +49,19 @@ static int are_vehicles_intersecting(
|
|||
return 0;
|
||||
}
|
||||
|
||||
int dcm_check(void* fc, DCM_t* dcm) {
|
||||
facilities_t* facilities = (facilities_t*) fc;
|
||||
|
||||
static int dcm_check_handle_request(facilities_t* facilities, DCM_t* dcm) {
|
||||
int rv = 0;
|
||||
|
||||
DCM_t* dcm_req = NULL;
|
||||
CoordinationRequest_t* coo_req = &dcm->dcm.negotiation->choice.request;
|
||||
|
||||
DCM_t* dcm_rep = NULL;
|
||||
TransportRequest_t* tr = NULL;
|
||||
|
||||
const ssize_t buf_len = 512;
|
||||
uint8_t buf[buf_len];
|
||||
|
||||
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;
|
||||
ssize_t trajectoryA_len = 0;
|
||||
|
||||
uint64_t now = it2s_tender_get_clock(&facilities->epv);
|
||||
|
||||
|
|
@ -84,35 +82,143 @@ int dcm_check(void* fc, DCM_t* dcm) {
|
|||
trajectoryA[0].timestamp = now;
|
||||
++trajectoryA_len;
|
||||
|
||||
trajectoryB[0].latitude = dcm->dcm.currentPosition.latitude;
|
||||
trajectoryB[0].longitude = dcm->dcm.currentPosition.longitude;
|
||||
asn_INTEGER2ulong(&dcm->dcm.currentPosition.timestamp, &trajectoryB[0].timestamp);
|
||||
++trajectoryB_len;
|
||||
for (int i = 0; i < dcm->dcm.plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) {
|
||||
trajectoryB[i+1].latitude = dcm->dcm.plannedTrajectory.list.array[i]->latitude;
|
||||
trajectoryB[i+1].longitude = dcm->dcm.plannedTrajectory.list.array[i]->longitude;
|
||||
asn_INTEGER2ulong(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, &trajectoryB[i+1].timestamp);
|
||||
++trajectoryB_len;
|
||||
dcm_rep = calloc(1, sizeof(DCM_t));
|
||||
|
||||
dcm_rep->header.messageID = 43;
|
||||
dcm_rep->header.protocolVersion = 1;
|
||||
dcm_rep->header.stationID = 1;
|
||||
pthread_mutex_lock(&facilities->id.lock);
|
||||
dcm_rep->header.stationID = facilities->id.station_id;
|
||||
pthread_mutex_unlock(&facilities->id.lock);
|
||||
|
||||
dcm_rep->dcm.currentPosition.latitude = lat;
|
||||
dcm_rep->dcm.currentPosition.longitude = lon;
|
||||
asn_ulong2INTEGER(&dcm_rep->dcm.currentPosition.timestamp, now);
|
||||
|
||||
// Vehicle Dimensions
|
||||
dcm_rep->dcm.vehicleDimensions.length= facilities->vehicle.length;
|
||||
dcm_rep->dcm.vehicleDimensions.width = facilities->vehicle.width;
|
||||
|
||||
// Accepted trajectory
|
||||
if (!coo_req->desiredTrajectories.list.count) {
|
||||
syslog_debug("[facilities] [dc] received DCM request has no desired trajectories");
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
ProposedTrajectory_t* pt = coo_req->desiredTrajectories.list.array[0];
|
||||
|
||||
dcm_rep->dcm.negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
|
||||
dcm_rep->dcm.negotiation->present = CoordinationNegotiation_PR_reply;
|
||||
dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectories.list.count = 1;
|
||||
dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectories.list.size = sizeof(void*);
|
||||
dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectories.list.array = malloc(1*sizeof(void*));
|
||||
dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectories.list.array[0] = calloc(1,sizeof(AcceptedTrajectory_t));
|
||||
AcceptedTrajectory_t* at = dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectories.list.array[0];
|
||||
|
||||
at->trajectory.list.count = pt->trajectory.list.count;
|
||||
at->trajectory.list.size = sizeof(void*) * pt->trajectory.list.count;
|
||||
at->trajectory.list.array = malloc(sizeof(void*) * pt->trajectory.list.count);
|
||||
for (int i = 0; i < pt->trajectory.list.count; ++i) {
|
||||
at->trajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
|
||||
at->trajectory.list.array[i]->latitude = pt->trajectory.list.array[i]->latitude;
|
||||
at->trajectory.list.array[i]->longitude = pt->trajectory.list.array[i]->longitude;
|
||||
at->trajectory.list.array[i]->timestamp.buf = malloc(pt->trajectory.list.array[i]->timestamp.size);
|
||||
memcpy(at->trajectory.list.array[i]->timestamp.buf, pt->trajectory.list.array[i]->timestamp.buf, pt->trajectory.list.array[i]->timestamp.size);
|
||||
at->trajectory.list.array[i]->timestamp.size = pt->trajectory.list.array[i]->timestamp.size;
|
||||
}
|
||||
|
||||
bool intersecting = false;
|
||||
int index = 0;
|
||||
at->id = pt->id;
|
||||
|
||||
if (trajectoryA_len > 1 && trajectoryB_len > 1) {
|
||||
intersecting = are_vehicles_intersecting(
|
||||
trajectoryA, trajectoryA_len, facilities->vehicle.length, facilities->vehicle.width,
|
||||
trajectoryB, trajectoryB_len, dcm->dcm.vehicleDimensions.length, dcm->dcm.vehicleDimensions.width,
|
||||
&index);
|
||||
asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_DCM, dcm_rep, buf, buf_len);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] [dc] DCM request encode failure");
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
if (!intersecting) goto cleanup;
|
||||
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;
|
||||
|
||||
syslog_info("[facilities] [dc] intersection danger with %ld @ (%d, %d) in %ld ms",
|
||||
dcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now);
|
||||
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);
|
||||
memcpy(bpr->data.buf, buf, buf_len);
|
||||
bpr->data.size = (enc.encoded + 7) / 8;
|
||||
|
||||
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] [dc] TR DCM request encode failure");
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
queue_send(facilities->tx_queue, buf, enc.encoded+1, 3);
|
||||
|
||||
cleanup:
|
||||
ASN_STRUCT_FREE(asn_DEF_DCM, dcm_rep);
|
||||
ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
|
||||
return rv;
|
||||
}
|
||||
|
||||
static int dcm_check_handle_reply(facilities_t* facilities, DCM_t* dcm) {
|
||||
int rv = 0;
|
||||
|
||||
CoordinationReply_t* coo_rep = &dcm->dcm.negotiation->choice.reply;
|
||||
|
||||
syslog_info("[facilities] [dc] received coordination reply with %d accepted trajectories", coo_rep->acceptedTrajectories.list.count);
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
static int dcm_check_intersection_detected(facilities_t* facilities, DCM_t* dcm) {
|
||||
|
||||
int rv = 0;
|
||||
|
||||
DCM_t* dcm_req = NULL;
|
||||
TransportRequest_t* tr = NULL;
|
||||
|
||||
const ssize_t buf_len = 512;
|
||||
uint8_t buf[buf_len];
|
||||
|
||||
it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
|
||||
uint16_t trajectoryA_len = 0;
|
||||
|
||||
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);
|
||||
|
||||
uint64_t now = it2s_tender_get_clock(&facilities->epv);
|
||||
|
||||
trajectoryA[0].latitude = lat;
|
||||
trajectoryA[0].longitude = lon;
|
||||
trajectoryA[0].timestamp = now;
|
||||
++trajectoryA_len;
|
||||
|
||||
// Initiate conflict resolution
|
||||
dcm_req = calloc(1, sizeof(DCM_t));
|
||||
it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN];
|
||||
|
||||
dcm_req->header.messageID = 43;
|
||||
dcm_req->header.protocolVersion = 1;
|
||||
|
|
@ -135,9 +241,9 @@ int dcm_check(void* fc, DCM_t* dcm) {
|
|||
dcm_req->dcm.plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*));
|
||||
for (int i = 0; i < trajectoryA_len; ++i) {
|
||||
dcm_req->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
|
||||
dcm_req->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i+1].latitude;
|
||||
dcm_req->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i+1].longitude;
|
||||
asn_ulong2INTEGER(&dcm_req->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i+1].timestamp);
|
||||
dcm_req->dcm.plannedTrajectory.list.array[i]->latitude = trajectoryA[i+1].latitude;
|
||||
dcm_req->dcm.plannedTrajectory.list.array[i]->longitude = trajectoryA[i+1].longitude;
|
||||
asn_ulong2INTEGER(&dcm_req->dcm.plannedTrajectory.list.array[i]->timestamp, trajectoryA[i+1].timestamp);
|
||||
}
|
||||
|
||||
// Desired trajectory
|
||||
|
|
@ -154,9 +260,9 @@ int dcm_check(void* fc, DCM_t* dcm) {
|
|||
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 = trajectory[i+1].latitude;
|
||||
pt->trajectory.list.array[i]->longitude = trajectory[i+1].longitude;
|
||||
asn_ulong2INTEGER(&pt->trajectory.list.array[i]->timestamp, trajectory[i+1].timestamp);
|
||||
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;
|
||||
|
|
@ -209,6 +315,79 @@ cleanup:
|
|||
return rv;
|
||||
}
|
||||
|
||||
int dcm_check(void* fc, DCM_t* dcm) {
|
||||
facilities_t* facilities = (facilities_t*) fc;
|
||||
|
||||
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);
|
||||
|
||||
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 = dcm->dcm.currentPosition.latitude;
|
||||
trajectoryB[0].longitude = dcm->dcm.currentPosition.longitude;
|
||||
asn_INTEGER2ulong(&dcm->dcm.currentPosition.timestamp, &trajectoryB[0].timestamp);
|
||||
++trajectoryB_len;
|
||||
for (int i = 0; i < dcm->dcm.plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) {
|
||||
trajectoryB[i+1].latitude = dcm->dcm.plannedTrajectory.list.array[i]->latitude;
|
||||
trajectoryB[i+1].longitude = dcm->dcm.plannedTrajectory.list.array[i]->longitude;
|
||||
asn_INTEGER2ulong(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, &trajectoryB[i+1].timestamp);
|
||||
++trajectoryB_len;
|
||||
}
|
||||
|
||||
bool intersecting = false;
|
||||
int index = 0;
|
||||
|
||||
if (dcm->dcm.negotiation) {
|
||||
switch (dcm->dcm.negotiation->present) {
|
||||
case CoordinationNegotiation_PR_request:
|
||||
dcm_check_handle_request(facilities, dcm);
|
||||
break;
|
||||
case CoordinationNegotiation_PR_reply:
|
||||
dcm_check_handle_reply(facilities, dcm);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
if (trajectoryA_len > 1 && trajectoryB_len > 1) {
|
||||
intersecting = are_vehicles_intersecting(
|
||||
trajectoryA, trajectoryA_len, facilities->vehicle.length, facilities->vehicle.width,
|
||||
trajectoryB, trajectoryB_len, dcm->dcm.vehicleDimensions.length, dcm->dcm.vehicleDimensions.width,
|
||||
&index);
|
||||
}
|
||||
|
||||
if (intersecting) {
|
||||
syslog_info("[facilities] [dc] intersection danger with %ld @ (%d, %d) in %ld ms",
|
||||
dcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now);
|
||||
dcm_check_intersection_detected(facilities, dcm);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
static int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len) {
|
||||
int rv = 0;
|
||||
|
||||
|
|
@ -309,7 +488,7 @@ void* dc_service(void* fc) {
|
|||
|
||||
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 1023);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] encoding TR for cam failed");
|
||||
syslog_err("[facilities] encoding TR for DCM failed");
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue