Changed DCM to PCM
This commit is contained in:
emanuel
parent
fe347cd24b
commit
fee7bd59a7
|
|
@ -9,7 +9,7 @@ ADD_EXECUTABLE(it2s-itss-facilities
|
|||
cpm.c
|
||||
saem.c
|
||||
tpm.c
|
||||
dcm.c
|
||||
pcm.c
|
||||
)
|
||||
|
||||
TARGET_LINK_LIBRARIES(it2s-itss-facilities
|
||||
|
|
@ -27,7 +27,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
|
|||
-lit2s-asn-cpm
|
||||
-lit2s-asn-saem
|
||||
-lit2s-asn-tpm
|
||||
-lit2s-asn-dcm
|
||||
-lit2s-asn-pcm
|
||||
-lit2s-tender
|
||||
-lm
|
||||
-lrt
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
#include "requests.h"
|
||||
#include "cpm.h"
|
||||
#include "saem.h"
|
||||
#include "dcm.h"
|
||||
#include "pcm.h"
|
||||
|
||||
#include <itss-networking/NetworkingIndication.h>
|
||||
#include <itss-transport/TransportRequest.h>
|
||||
|
|
@ -123,8 +123,8 @@ static int transport_indication(facilities_t *facilities, void* responder, void*
|
|||
break;
|
||||
|
||||
case 2043: /* maneuvers */
|
||||
its_msg_descriptor = &asn_DEF_DCM;
|
||||
its_msg = calloc(1, sizeof(DCM_t));
|
||||
its_msg_descriptor = &asn_DEF_PCM;
|
||||
its_msg = calloc(1, sizeof(PCM_t));
|
||||
handled_msg = true;
|
||||
break;
|
||||
|
||||
|
|
@ -223,7 +223,7 @@ static int transport_indication(facilities_t *facilities, void* responder, void*
|
|||
break;
|
||||
|
||||
case 2043:
|
||||
dcm_check(facilities, its_msg);
|
||||
pcm_check(facilities, its_msg);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -710,7 +710,7 @@ int main() {
|
|||
|
||||
// DC
|
||||
if (facilities.coordination.active)
|
||||
pthread_create(&facilities.dc_service, NULL, dc_service, (void*) &facilities);
|
||||
pthread_create(&facilities.pc_service, NULL, pc_service, (void*) &facilities);
|
||||
|
||||
void* security_socket = zmq_socket(facilities.zmq.ctx, ZMQ_REQ);
|
||||
int wait_ms = 1000;
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@
|
|||
#include "cpm.h"
|
||||
#include "saem.h"
|
||||
#include "tpm.h"
|
||||
#include "dcm.h"
|
||||
#include "pcm.h"
|
||||
|
||||
#include <it2s-tender/epv.h>
|
||||
#include <it2s-tender/syslog.h>
|
||||
|
|
@ -31,7 +31,7 @@ typedef struct facilities {
|
|||
pthread_t transmitting;
|
||||
pthread_t cp_service;
|
||||
pthread_t sa_service;
|
||||
pthread_t dc_service;
|
||||
pthread_t pc_service;
|
||||
|
||||
// ZMQ
|
||||
struct {
|
||||
|
|
|
|||
|
|
@ -1,13 +1,12 @@
|
|||
#include "dcm.h"
|
||||
#include "pcm.h"
|
||||
#include "facilities.h"
|
||||
|
||||
#include <dcm/asn_application.h>
|
||||
#include <it2s-tender/epv.h>
|
||||
#include <it2s-tender/space.h>
|
||||
#include <it2s-tender/trajectory.h>
|
||||
#include <it2s-tender/geodesy.h>
|
||||
#include <itss-transport/TransportRequest.h>
|
||||
#include <dcm/DCM.h>
|
||||
#include <pcm/PCM.h>
|
||||
|
||||
#define SLEEP_TIME_MS 100
|
||||
|
||||
|
|
@ -49,12 +48,12 @@ static int are_vehicles_intersecting(
|
|||
return 0;
|
||||
}
|
||||
|
||||
static int dcm_check_handle_request(facilities_t* facilities, DCM_t* dcm) {
|
||||
static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm) {
|
||||
int rv = 0;
|
||||
|
||||
CoordinationRequest_t* coo_req = &dcm->dcm.negotiation->choice.request;
|
||||
CoordinationRequest_t* request = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.request;
|
||||
|
||||
DCM_t* dcm_rep = NULL;
|
||||
PCM_t* pcm_rep = NULL;
|
||||
TransportRequest_t* tr = NULL;
|
||||
|
||||
const ssize_t buf_len = 512;
|
||||
|
|
@ -82,43 +81,46 @@ static int dcm_check_handle_request(facilities_t* facilities, DCM_t* dcm) {
|
|||
trajectoryA[0].timestamp = now;
|
||||
++trajectoryA_len;
|
||||
|
||||
dcm_rep = calloc(1, sizeof(DCM_t));
|
||||
pcm_rep = calloc(1, sizeof(PCM_t));
|
||||
|
||||
dcm_rep->header.messageID = 43;
|
||||
dcm_rep->header.protocolVersion = 1;
|
||||
dcm_rep->header.stationID = 1;
|
||||
pcm_rep->header.messageID = 43;
|
||||
pcm_rep->header.protocolVersion = 1;
|
||||
pcm_rep->header.stationID = 1;
|
||||
pthread_mutex_lock(&facilities->id.lock);
|
||||
dcm_rep->header.stationID = facilities->id.station_id;
|
||||
pcm_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);
|
||||
pcm_rep->pcm.currentPosition.latitude = lat;
|
||||
pcm_rep->pcm.currentPosition.longitude = lon;
|
||||
asn_ulong2INTEGER(&pcm_rep->pcm.currentPosition.timestamp, now);
|
||||
|
||||
pcm_rep->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
|
||||
ManeuverVehicleContainer_t* mvc = &pcm_rep->pcm.maneuverContainer.choice.vehicle;
|
||||
|
||||
// Vehicle DimensionsDimensions
|
||||
dcm_rep->dcm.vehicleLength.vehicleLengthValue= facilities->vehicle.length;
|
||||
dcm_rep->dcm.vehicleLength.vehicleLengthConfidenceIndication = 0;
|
||||
dcm_rep->dcm.vehicleWidth = facilities->vehicle.width;
|
||||
mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
|
||||
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
|
||||
mvc->vehicleWidth = facilities->vehicle.width;
|
||||
|
||||
// Accepted trajectory
|
||||
if (!coo_req->desiredTrajectories.list.count) {
|
||||
syslog_debug("[facilities] [dc] received DCM request has no desired trajectories");
|
||||
if (!request->desiredTrajectories.list.count) {
|
||||
syslog_debug("[facilities] [pc] received PCM request has no desired trajectories");
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
ProposedTrajectory_t* pt = coo_req->desiredTrajectories.list.array[0];
|
||||
ProposedTrajectory_t* pt = request->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.acceptedTrajectoriesIds.list.count = 1;
|
||||
dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectoriesIds.list.size = sizeof(void*);
|
||||
dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectoriesIds.list.array = malloc(1*sizeof(void*));
|
||||
dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = malloc(sizeof(long));
|
||||
*dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = pt->id;
|
||||
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;
|
||||
|
||||
asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_DCM, dcm_rep, buf, buf_len);
|
||||
asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_PCM, pcm_rep, buf, buf_len);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] [dc] DCM request encode failure");
|
||||
syslog_err("[facilities] [pc] PCM request encode failure");
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
|
@ -149,7 +151,7 @@ static int dcm_check_handle_request(facilities_t* facilities, DCM_t* dcm) {
|
|||
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");
|
||||
syslog_err("[facilities] [pc] TR PCM request encode failure");
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
|
@ -157,26 +159,26 @@ static int dcm_check_handle_request(facilities_t* facilities, DCM_t* dcm) {
|
|||
queue_send(facilities->tx_queue, buf, enc.encoded+1, 3);
|
||||
|
||||
cleanup:
|
||||
ASN_STRUCT_FREE(asn_DEF_DCM, dcm_rep);
|
||||
ASN_STRUCT_FREE(asn_DEF_PCM, pcm_rep);
|
||||
ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
|
||||
return rv;
|
||||
}
|
||||
|
||||
static int dcm_check_handle_reply(facilities_t* facilities, DCM_t* dcm) {
|
||||
static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm) {
|
||||
int rv = 0;
|
||||
|
||||
CoordinationReply_t* coo_rep = &dcm->dcm.negotiation->choice.reply;
|
||||
CoordinationReply_t* reply = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.reply;
|
||||
|
||||
syslog_info("[facilities] [dc] received coordination reply with %d accepted trajectories", coo_rep->acceptedTrajectoriesIds.list.count);
|
||||
syslog_info("[facilities] [pc] received coordination reply with %d accepted trajectories", reply->acceptedTrajectoriesIds.list.count);
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
static int dcm_check_intersection_detected(facilities_t* facilities, DCM_t* dcm) {
|
||||
static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm) {
|
||||
|
||||
int rv = 0;
|
||||
|
||||
DCM_t* dcm_req = NULL;
|
||||
PCM_t* pcm_req = NULL;
|
||||
TransportRequest_t* tr = NULL;
|
||||
|
||||
const ssize_t buf_len = 512;
|
||||
|
|
@ -205,43 +207,45 @@ static int dcm_check_intersection_detected(facilities_t* facilities, DCM_t* dcm)
|
|||
++trajectoryA_len;
|
||||
|
||||
// Initiate conflict resolution
|
||||
dcm_req = calloc(1, sizeof(DCM_t));
|
||||
pcm_req = calloc(1, sizeof(PCM_t));
|
||||
|
||||
dcm_req->header.messageID = 43;
|
||||
dcm_req->header.protocolVersion = 1;
|
||||
dcm_req->header.stationID = 1;
|
||||
pcm_req->header.messageID = 43;
|
||||
pcm_req->header.protocolVersion = 1;
|
||||
pcm_req->header.stationID = 1;
|
||||
pthread_mutex_lock(&facilities->id.lock);
|
||||
dcm_req->header.stationID = facilities->id.station_id;
|
||||
pcm_req->header.stationID = facilities->id.station_id;
|
||||
pthread_mutex_unlock(&facilities->id.lock);
|
||||
|
||||
dcm_req->dcm.currentPosition.latitude = lat;
|
||||
dcm_req->dcm.currentPosition.longitude = lon;
|
||||
asn_ulong2INTEGER(&dcm_req->dcm.currentPosition.timestamp, now);
|
||||
pcm_req->pcm.currentPosition.latitude = lat;
|
||||
pcm_req->pcm.currentPosition.longitude = lon;
|
||||
asn_ulong2INTEGER(&pcm_req->pcm.currentPosition.timestamp, now);
|
||||
|
||||
ManeuverVehicleContainer_t* mvc = &pcm_req->pcm.maneuverContainer.choice.vehicle;
|
||||
|
||||
// Vehicle Dimensions
|
||||
dcm_req->dcm.vehicleLength.vehicleLengthValue= facilities->vehicle.length;
|
||||
dcm_req->dcm.vehicleLength.vehicleLengthConfidenceIndication = 0;
|
||||
dcm_req->dcm.vehicleWidth = facilities->vehicle.width;
|
||||
mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
|
||||
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
|
||||
mvc->vehicleWidth = facilities->vehicle.width;
|
||||
|
||||
// Planned trajectory
|
||||
dcm_req->dcm.plannedTrajectory.list.count = --trajectoryA_len;
|
||||
dcm_req->dcm.plannedTrajectory.list.size = trajectoryA_len * sizeof(void*);
|
||||
dcm_req->dcm.plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*));
|
||||
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) {
|
||||
dcm_req->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
|
||||
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);
|
||||
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
|
||||
dcm_req->dcm.negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
|
||||
dcm_req->dcm.negotiation->present = CoordinationNegotiation_PR_request;
|
||||
dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.count = 1;
|
||||
dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.size = sizeof(void*);
|
||||
dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.array = malloc(1*sizeof(void*));
|
||||
dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t));
|
||||
ProposedTrajectory_t* pt = dcm_req->dcm.negotiation->choice.request.desiredTrajectories.list.array[0];
|
||||
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];
|
||||
|
||||
pt->trajectory.list.count = trajectoryA_len;
|
||||
pt->trajectory.list.size = sizeof(void*) * trajectoryA_len;
|
||||
|
|
@ -257,9 +261,9 @@ static int dcm_check_intersection_detected(facilities_t* facilities, DCM_t* dcm)
|
|||
pt->priority = 1;
|
||||
pt->id = 0;
|
||||
|
||||
asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_DCM, dcm_req, buf, buf_len);
|
||||
asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_PCM, pcm_req, buf, buf_len);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] [dc] DCM request encode failure");
|
||||
syslog_err("[facilities] [pc] PCM request encode failure");
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
|
@ -290,7 +294,7 @@ static int dcm_check_intersection_detected(facilities_t* facilities, DCM_t* dcm)
|
|||
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");
|
||||
syslog_err("[facilities] [pc] TR PCM request encode failure");
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
|
@ -298,12 +302,12 @@ static int dcm_check_intersection_detected(facilities_t* facilities, DCM_t* dcm)
|
|||
queue_send(facilities->tx_queue, buf, enc.encoded+1, 3);
|
||||
|
||||
cleanup:
|
||||
ASN_STRUCT_FREE(asn_DEF_DCM, dcm_req);
|
||||
ASN_STRUCT_FREE(asn_DEF_PCM, pcm_req);
|
||||
ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
|
||||
return rv;
|
||||
}
|
||||
|
||||
int dcm_check(void* fc, DCM_t* dcm) {
|
||||
int pcm_check(void* fc, PCM_t* pcm) {
|
||||
facilities_t* facilities = (facilities_t*) fc;
|
||||
coordination_s* coordination = &facilities->coordination;
|
||||
|
||||
|
|
@ -323,36 +327,12 @@ int dcm_check(void* fc, DCM_t* dcm) {
|
|||
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;
|
||||
|
||||
pthread_mutex_lock(&coordination->lock);
|
||||
|
||||
// Find neighbours
|
||||
int ni = -1;
|
||||
for (int i = 0; i < coordination->neighbours_len; ++i) {
|
||||
if (coordination->neighbours->station_id == dcm->header.stationID) {
|
||||
if (coordination->neighbours->station_id == pcm->header.stationID) {
|
||||
ni = i;
|
||||
break;
|
||||
}
|
||||
|
|
@ -360,37 +340,73 @@ int dcm_check(void* fc, DCM_t* dcm) {
|
|||
|
||||
// Add it if not found
|
||||
if (ni == -1 && coordination->neighbours_len + 1 < MC_MAX_NEIGHBOURS) {
|
||||
coordination->neighbours[coordination->neighbours_len].station_id = dcm->header.stationID;
|
||||
coordination->neighbours[coordination->neighbours_len].station_id = pcm->header.stationID;
|
||||
ni = coordination->neighbours_len;
|
||||
++coordination->neighbours_len;
|
||||
}
|
||||
|
||||
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.vehicleLength.vehicleLengthValue, dcm->dcm.vehicleWidth,
|
||||
&index);
|
||||
}
|
||||
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);
|
||||
break;
|
||||
case CoordinationNegotiation_PR_reply:
|
||||
pcm_check_handle_reply(facilities, pcm);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
|
||||
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);
|
||||
coordination->neighbours[ni].intersecting = true;
|
||||
coordination->neighbours[ni].t_iid = now;
|
||||
dcm_check_intersection_detected(facilities, dcm);
|
||||
}
|
||||
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 @ (%d, %d) in %ld ms",
|
||||
pcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now);
|
||||
coordination->neighbours[ni].intersecting = true;
|
||||
coordination->neighbours[ni].t_iid = now;
|
||||
pcm_check_intersection_detected(facilities, pcm);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case ManeuverContainer_PR_rsu:
|
||||
break;
|
||||
default:
|
||||
syslog_debug("[facilities] [pc] received PCM contains unrecognized ManeuverContainer type");
|
||||
rv = 1;
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&coordination->lock);
|
||||
|
|
@ -398,18 +414,18 @@ int dcm_check(void* fc, DCM_t* dcm) {
|
|||
return rv;
|
||||
}
|
||||
|
||||
static int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len) {
|
||||
static int mk_pcm(facilities_t* facilities, uint8_t* pcm_uper, uint16_t* pcm_uper_len) {
|
||||
int rv = 0;
|
||||
|
||||
it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN];
|
||||
|
||||
DCM_t* dcm = calloc(1, sizeof(DCM_t));
|
||||
PCM_t* pcm = calloc(1, sizeof(PCM_t));
|
||||
|
||||
dcm->header.messageID = 43;
|
||||
dcm->header.protocolVersion = 1;
|
||||
pcm->header.messageID = 43;
|
||||
pcm->header.protocolVersion = 1;
|
||||
uint64_t now = it2s_tender_get_clock(&facilities->epv);
|
||||
pthread_mutex_lock(&facilities->id.lock);
|
||||
dcm->header.stationID = facilities->id.station_id;
|
||||
pcm->header.stationID = facilities->id.station_id;
|
||||
pthread_mutex_unlock(&facilities->id.lock);
|
||||
|
||||
int32_t lat, lon;
|
||||
|
|
@ -425,45 +441,53 @@ static int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_upe
|
|||
memcpy(trajectory, facilities->epv.trajectory.path, trajectory_len * sizeof(it2s_tender_st_s));
|
||||
it2s_tender_unlock_trajectory(&facilities->epv);
|
||||
|
||||
dcm->dcm.currentPosition.latitude = lat;
|
||||
dcm->dcm.currentPosition.longitude = lon;
|
||||
asn_ulong2INTEGER(&dcm->dcm.currentPosition.timestamp, now);
|
||||
pcm->pcm.currentPosition.latitude = lat;
|
||||
pcm->pcm.currentPosition.longitude = lon;
|
||||
asn_ulong2INTEGER(&pcm->pcm.currentPosition.timestamp, now);
|
||||
|
||||
// Vehicle Dimensions
|
||||
dcm->dcm.vehicleLength.vehicleLengthValue= facilities->vehicle.length;
|
||||
dcm->dcm.vehicleLength.vehicleLengthConfidenceIndication = 0;
|
||||
dcm->dcm.vehicleWidth = facilities->vehicle.width;
|
||||
if (facilities->station_type == 15) {
|
||||
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;
|
||||
|
||||
dcm->dcm.plannedTrajectory.list.count = trajectory_len;
|
||||
dcm->dcm.plannedTrajectory.list.size = trajectory_len * sizeof(void*);
|
||||
dcm->dcm.plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*));
|
||||
for (int i = 0; i < trajectory_len; ++i) {
|
||||
dcm->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
|
||||
dcm->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i].latitude;
|
||||
dcm->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i].longitude;
|
||||
asn_ulong2INTEGER(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i].timestamp);
|
||||
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_DCM, NULL, dcm, dcm_uper, 512);
|
||||
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_PCM, NULL, pcm, pcm_uper, 512);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] [dc] failed encoding DCM (%s)", enc.failed_type->name);
|
||||
syslog_err("[facilities] [pc] failed encoding PCM (%s)", enc.failed_type->name);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
*dcm_uper_len = (enc.encoded + 7) / 8;
|
||||
*pcm_uper_len = (enc.encoded + 7) / 8;
|
||||
|
||||
cleanup:
|
||||
ASN_STRUCT_FREE(asn_DEF_DCM, dcm);
|
||||
ASN_STRUCT_FREE(asn_DEF_PCM, pcm);
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
void* dc_service(void* fc) {
|
||||
void* pc_service(void* fc) {
|
||||
|
||||
facilities_t* facilities = (facilities_t*) fc;
|
||||
coordination_s* coordination = (coordination_s*) &facilities->coordination;
|
||||
|
||||
uint8_t dcm[512];
|
||||
uint8_t pcm[512];
|
||||
|
||||
TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t));
|
||||
tr->present = TransportRequest_PR_packet;
|
||||
|
|
@ -493,14 +517,14 @@ void* dc_service(void* fc) {
|
|||
|
||||
while (!facilities->exit) {
|
||||
|
||||
rv = mk_dcm(facilities, bpr->data.buf, (uint16_t *) &bpr->data.size);
|
||||
rv = mk_pcm(facilities, bpr->data.buf, (uint16_t *) &bpr->data.size);
|
||||
if (rv) {
|
||||
continue;
|
||||
}
|
||||
|
||||
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 DCM failed");
|
||||
syslog_err("[facilities] encoding TR for PCM failed");
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -1,7 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <dcm/DCM.h>
|
||||
#include <pcm/PCM.h>
|
||||
|
||||
#define MC_MAX_NEIGHBOURS 32
|
||||
#define MC_RESOLUTION_TIMEOUT 1000
|
||||
|
|
@ -23,6 +23,6 @@ typedef struct coordination {
|
|||
|
||||
} coordination_s;
|
||||
|
||||
int dcm_check(void* fc, DCM_t* dcm);
|
||||
void* dc_service(void* fc);
|
||||
int pcm_check(void* fc, PCM_t* pcm);
|
||||
void* pc_service(void* fc);
|
||||
void coordination_init(coordination_s* coordination);
|
||||
Loading…
Reference in New Issue