diff --git a/src/dcm.c b/src/dcm.c index 000256a..e85046e 100644 --- a/src/dcm.c +++ b/src/dcm.c @@ -51,6 +51,8 @@ static int are_vehicles_intersecting( int dcm_check(void* fc, DCM_t* dcm) { facilities_t* facilities = (facilities_t*) fc; + DCM_t* dcm_req = NULL; + 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; @@ -86,19 +88,76 @@ int dcm_check(void* fc, DCM_t* dcm) { ++trajectoryB_len; } + bool intersecting = false; + int index = 0; + if (trajectoryA_len > 1 && trajectoryB_len > 1) { - int index = 0; - if (are_vehicles_intersecting( + 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) - ) { - 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); - } + &index); } + if (!intersecting) goto cleanup; + + 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); + + // Initiate conflict resolution + dcm = calloc(1, sizeof(DCM_t)); + it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN]; + + dcm->header.messageID = 43; + dcm->header.protocolVersion = 1; + dcm->header.stationID = 1; + pthread_mutex_lock(&facilities->id.lock); + dcm->header.stationID = facilities->id.station_id; + pthread_mutex_unlock(&facilities->id.lock); + + dcm->dcm.currentPosition.latitude = lat; + dcm->dcm.currentPosition.longitude = lon; + asn_ulong2INTEGER(&dcm->dcm.currentPosition.timestamp, now); + + // Vehicle Dimensions + dcm->dcm.vehicleDimensions.length= facilities->vehicle.length; + dcm->dcm.vehicleDimensions.width = facilities->vehicle.width; + + // Planned trajectory + dcm->dcm.plannedTrajectory.list.count = --trajectoryA_len; + dcm->dcm.plannedTrajectory.list.size = trajectoryA_len * sizeof(void*); + dcm->dcm.plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*)); + for (int i = 0; i < trajectoryA_len; ++i) { + dcm->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); + dcm->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i+1].latitude; + dcm->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i+1].longitude; + asn_ulong2INTEGER(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i+1].timestamp); + } + + // Desired trajectory + dcm->dcm.negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); + dcm->dcm.negotiation->present = CoordinationNegotiation_PR_request; + dcm->dcm.negotiation->choice.request.desiredTrajectories.list.count = 1; + dcm->dcm.negotiation->choice.request.desiredTrajectories.list.size = sizeof(void*); + dcm->dcm.negotiation->choice.request.desiredTrajectories.list.array = malloc(1*sizeof(void*)); + dcm->dcm.negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t)); + ProposedTrajectory_t* pt = dcm->dcm.negotiation->choice.request.desiredTrajectories.list.array[0]; + + 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 = 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->offer = 5; + pt->priority = 1; + pt->id = 0; + cleanup: + ASN_STRUCT_FREE(asn_DEF_DCM, dcm_req); return 0; }