This commit is contained in:
emanuel 2022-06-24 14:33:37 +01:00
parent 410ca6ea8e
commit cd293f1309
6 changed files with 127 additions and 127 deletions

View File

@ -9,7 +9,7 @@ ADD_EXECUTABLE(it2s-itss-facilities
cpm.c
saem.c
tpm.c
pcm.c
vcm.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-pcm
-lit2s-asn-vcm
-lit2s-tender
-lm
-lrt

View File

@ -317,10 +317,10 @@ int facilities_config(void* facilities_s) {
}
facilities->tolling.station.obu.client_id = config->facilities.tpm.client_id;
// PCM
// VCM
facilities->coordination.active = config->facilities.dcm.activate;
facilities->coordination.pcm_period_min = config->facilities.dcm.period_min;
facilities->coordination.pcm_period_max = config->facilities.dcm.period_max;
facilities->coordination.vcm_period_min = config->facilities.dcm.period_min;
facilities->coordination.vcm_period_max = config->facilities.dcm.period_max;
// Replay
facilities->replay = config->networking.replay.activate;

View File

@ -7,7 +7,7 @@
#include "requests.h"
#include "cpm.h"
#include "saem.h"
#include "pcm.h"
#include "vcm.h"
#include <itss-networking/NetworkingIndication.h>
#include <itss-transport/TransportRequest.h>
@ -139,8 +139,8 @@ static int transport_indication(facilities_t *facilities, void* responder, void*
break;
case 2043: /* maneuvers */
its_msg_descriptor = &asn_DEF_PCM;
its_msg = calloc(1, sizeof(PCM_t));
its_msg_descriptor = &asn_DEF_VCM;
its_msg = calloc(1, sizeof(VCM_t));
its_msg_type = 43;
handled_msg = true;
break;
@ -246,7 +246,7 @@ static int transport_indication(facilities_t *facilities, void* responder, void*
case 2043:
if (facilities->coordination.active) {
pcm_check(facilities, its_msg);
vcm_check(facilities, its_msg);
}
fwd = true;
break;
@ -811,7 +811,7 @@ int main() {
// PC
if (facilities.coordination.active)
pthread_create(&facilities.pc_service, NULL, pc_service, (void*) &facilities);
pthread_create(&facilities.vc_service, NULL, vc_service, (void*) &facilities);
void* security_socket = zmq_socket(facilities.zmq.ctx, ZMQ_REQ);

View File

@ -11,7 +11,7 @@
#include "cpm.h"
#include "saem.h"
#include "tpm.h"
#include "pcm.h"
#include "vcm.h"
#include <it2s-tender/epv.h>
#include <it2s-tender/syslog.h>
@ -33,7 +33,7 @@ typedef struct facilities {
pthread_t transmitting;
pthread_t cp_service;
pthread_t sa_service;
pthread_t pc_service;
pthread_t vc_service;
// ZMQ
struct {

View File

@ -1,4 +1,4 @@
#include "pcm.h"
#include "vcm.h"
#include "facilities.h"
#include <it2s-tender/epv.h>
@ -8,7 +8,7 @@
#include <it2s-tender/recorder.h>
#include <itss-transport/TransportRequest.h>
#include <itss-facilities/FacilitiesIndication.h>
#include <pcm/PCM.h>
#include <vcm/VCM.h>
static int are_vehicles_intersecting(
it2s_tender_st_s* tA, int tA_len, uint16_t vA_length, uint16_t vA_width,
@ -48,14 +48,14 @@ static int are_vehicles_intersecting(
return 0;
}
static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_neighbour_s* neighbour) {
static int vcm_check_handle_request(facilities_t* facilities, VCM_t* vcm, mc_neighbour_s* neighbour) {
int rv = 0;
coordination_s* coordination = &facilities->coordination;
CoordinationRequest_t* request = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.request;
CoordinationRequest_t* request = &vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.request;
PCM_t* pcm_rep = NULL;
VCM_t* vcm_rep = NULL;
TransportRequest_t* tr = NULL;
FacilitiesIndication_t* fi = NULL;
@ -87,30 +87,30 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei
trajectoryA[0].timestamp = now;
++trajectoryA_len;
pcm_rep = calloc(1, sizeof(PCM_t));
vcm_rep = calloc(1, sizeof(VCM_t));
pcm_rep->header.messageID = 43;
pcm_rep->header.protocolVersion = 1;
pcm_rep->header.stationID = 1;
vcm_rep->header.messageID = 43;
vcm_rep->header.protocolVersion = 1;
vcm_rep->header.stationID = 1;
pthread_mutex_lock(&facilities->id.lock);
pcm_rep->header.stationID = facilities->id.station_id;
vcm_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);
vcm_rep->vcm.currentPosition.latitude = lat;
vcm_rep->vcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&vcm_rep->vcm.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;
vcm_rep->vcm.chain = calloc(1, sizeof(ChainInformation_t));
vcm_rep->vcm.chain->id = coordination->chain.id;
vcm_rep->vcm.chain->link.buf = malloc(8);
vcm_rep->vcm.chain->link.size = 8;
memcpy(vcm_rep->vcm.chain->link.buf, coordination->chain.link, 8);
vcm_rep->vcm.chain->area.present = ChainGeoRegion_PR_circle;
}
pcm_rep->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &pcm_rep->pcm.maneuverContainer.choice.vehicle;
vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &vcm_rep->vcm.maneuverContainer.choice.vehicle;
// Vehicle DimensionsDimensions
mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
@ -119,7 +119,7 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei
// Accepted trajectory
if (!request->desiredTrajectories.list.count) {
syslog_debug("[facilities] [pc] received PCM request has no desired trajectories");
syslog_debug("[facilities] [vc] received VCM request has no desired trajectories");
rv = 1;
goto cleanup;
}
@ -132,15 +132,15 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei
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;
mvc->negotiation->choice.reply.requesterId = vcm->header.stationID;
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_PCM, NULL, pcm_rep, buf, buf_len);
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm_rep, buf, buf_len);
if (enc.encoded == -1) {
syslog_err("[facilities] [pc] PCM.reply encode failure (%s)", enc.failed_type->name);
syslog_err("[facilities] [vc] VCM.reply encode failure (%s)", enc.failed_type->name);
rv = 1;
goto cleanup;
}
ssize_t pcm_rep_len = (enc.encoded + 7) / 8;
ssize_t vcm_rep_len = (enc.encoded + 7) / 8;
tr = calloc(1, sizeof(TransportRequest_t));
tr->present = TransportRequest_PR_packet;
@ -156,13 +156,13 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei
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;
bpr->data.buf = malloc(vcm_rep_len);
memcpy(bpr->data.buf, buf, vcm_rep_len);
bpr->data.size = vcm_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);
syslog_err("[facilities] [vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
rv = 1;
goto cleanup;
}
@ -179,7 +179,7 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei
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);
syslog_err("[facilities] [vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
rv = 1;
goto cleanup;
}
@ -204,25 +204,25 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei
}
cleanup:
ASN_STRUCT_FREE(asn_DEF_PCM, pcm_rep);
ASN_STRUCT_FREE(asn_DEF_VCM, vcm_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) {
static int vcm_check_handle_reply(facilities_t* facilities, VCM_t* vcm, mc_neighbour_s* neighbour) {
int rv = 0;
CoordinationReply_t* reply = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.reply;
CoordinationReply_t* reply = &vcm->vcm.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);
syslog_info("[facilities] [vc] received VCM.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");
syslog_info("[facilities] [vc] received VCM.reply is response to another ITS-S VCM.request");
}
@ -232,13 +232,13 @@ static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm, mc_neigh
return rv;
}
static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm, mc_neighbour_s* neighbour) {
static int vcm_check_intersection_detected(facilities_t* facilities, VCM_t* vcm, mc_neighbour_s* neighbour) {
int rv = 0;
coordination_s* coordination = &facilities->coordination;
PCM_t* pcm_req = NULL;
VCM_t* vcm_req = NULL;
TransportRequest_t* tr = NULL;
FacilitiesIndication_t* fi = NULL;
@ -286,30 +286,30 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm,
++trajectoryA_len;
// Initiate conflict resolution
pcm_req = calloc(1, sizeof(PCM_t));
vcm_req = calloc(1, sizeof(VCM_t));
pcm_req->header.messageID = 43;
pcm_req->header.protocolVersion = 1;
pcm_req->header.stationID = 1;
vcm_req->header.messageID = 43;
vcm_req->header.protocolVersion = 1;
vcm_req->header.stationID = 1;
pthread_mutex_lock(&facilities->id.lock);
pcm_req->header.stationID = facilities->id.station_id;
vcm_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);
vcm_req->vcm.currentPosition.latitude = lat;
vcm_req->vcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&vcm_req->vcm.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;
vcm_req->vcm.chain = calloc(1, sizeof(ChainInformation_t));
vcm_req->vcm.chain->id = coordination->chain.id;
vcm_req->vcm.chain->link.buf = malloc(8);
vcm_req->vcm.chain->link.size = 8;
memcpy(vcm_req->vcm.chain->link.buf, coordination->chain.link, 8);
vcm_req->vcm.chain->area.present = ChainGeoRegion_PR_circle;
}
pcm_req->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &pcm_req->pcm.maneuverContainer.choice.vehicle;
vcm_req->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &vcm_req->vcm.maneuverContainer.choice.vehicle;
// Vehicle Dimensions
mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
@ -335,7 +335,7 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm,
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;
mvc->negotiation->choice.request.requesterId = vcm->header.stationID;
pt->trajectory.list.count = trajectoryA_len;
pt->trajectory.list.size = sizeof(void*) * trajectoryA_len;
@ -351,13 +351,13 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm,
pt->priority = 1;
pt->id = 0;
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_PCM, NULL, pcm_req, buf, buf_len);
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm_req, buf, buf_len);
if (enc.encoded == -1) {
syslog_err("[facilities] [pc] PCM.request encode failure (%s)", enc.failed_type->name);
syslog_err("[facilities] [vc] VCM.request encode failure (%s)", enc.failed_type->name);
rv = 1;
goto cleanup;
}
ssize_t pcm_req_len = (enc.encoded + 7) / 8;
ssize_t vcm_req_len = (enc.encoded + 7) / 8;
tr = calloc(1, sizeof(TransportRequest_t));
tr->present = TransportRequest_PR_packet;
@ -373,13 +373,13 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm,
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;
bpr->data.buf = malloc(vcm_req_len);
memcpy(bpr->data.buf, buf, vcm_req_len);
bpr->data.size = vcm_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);
syslog_err("[facilities] [vc] TR VCM.request encode failure (%s)", enc.failed_type->name);
rv = 1;
goto cleanup;
}
@ -396,7 +396,7 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm,
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);
syslog_err("[facilities] [vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
rv = 1;
goto cleanup;
}
@ -421,13 +421,13 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm,
}
cleanup:
ASN_STRUCT_FREE(asn_DEF_PCM, pcm_req);
ASN_STRUCT_FREE(asn_DEF_VCM, vcm_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) {
int vcm_check(void* fc, VCM_t* vcm) {
facilities_t* facilities = (facilities_t*) fc;
coordination_s* coordination = &facilities->coordination;
@ -452,7 +452,7 @@ int pcm_check(void* fc, PCM_t* pcm) {
// Find neighbours
int ni = -1;
for (int i = 0; i < coordination->neighbours_len; ++i) {
if (coordination->neighbours->station_id == pcm->header.stationID) {
if (coordination->neighbours->station_id == vcm->header.stationID) {
ni = i;
break;
}
@ -460,22 +460,22 @@ int pcm_check(void* fc, PCM_t* pcm) {
// 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;
coordination->neighbours[coordination->neighbours_len].station_id = vcm->header.stationID;
ni = coordination->neighbours_len;
++coordination->neighbours_len;
}
switch (pcm->pcm.maneuverContainer.present) {
switch (vcm->vcm.maneuverContainer.present) {
case ManeuverContainer_PR_vehicle:
;
ManeuverVehicleContainer_t* mvc = &pcm->pcm.maneuverContainer.choice.vehicle;
ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
if (mvc->negotiation) {
switch (mvc->negotiation->present) {
case CoordinationNegotiation_PR_request:
pcm_check_handle_request(facilities, pcm, &coordination->neighbours[ni]);
vcm_check_handle_request(facilities, vcm, &coordination->neighbours[ni]);
break;
case CoordinationNegotiation_PR_reply:
pcm_check_handle_reply(facilities, pcm, &coordination->neighbours[ni]);
vcm_check_handle_reply(facilities, vcm, &coordination->neighbours[ni]);
break;
default:
break;
@ -492,9 +492,9 @@ int pcm_check(void* fc, PCM_t* pcm) {
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, (unsigned long long*) &trajectoryB[0].timestamp);
trajectoryB[0].latitude = vcm->vcm.currentPosition.latitude;
trajectoryB[0].longitude = vcm->vcm.currentPosition.longitude;
asn_INTEGER2ulong(&vcm->vcm.currentPosition.timestamp, (unsigned long long*) &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;
@ -514,20 +514,20 @@ int pcm_check(void* fc, PCM_t* pcm) {
}
if (intersecting) {
syslog_info("[facilities] [pc] intersection danger with %ld @ (%f, %f) in %ld ms",
pcm->header.stationID,
syslog_info("[facilities] [vc] intersection danger with %ld @ (%f, %f) in %ld ms",
vcm->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]);
vcm_check_intersection_detected(facilities, vcm, &coordination->neighbours[ni]);
}
}
break;
case ManeuverContainer_PR_rsu:
break;
default:
syslog_debug("[facilities] [pc] received PCM contains unrecognized ManeuverContainer type");
syslog_debug("[facilities] [vc] received VCM contains unrecognized ManeuverContainer type");
rv = 1;
}
@ -536,20 +536,20 @@ int pcm_check(void* fc, PCM_t* pcm) {
return rv;
}
static int mk_pcm(facilities_t* facilities, uint8_t* pcm_uper, uint16_t* pcm_uper_len) {
static int mk_vcm(facilities_t* facilities, uint8_t* vcm_uper, uint16_t* vcm_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));
VCM_t* vcm = calloc(1, sizeof(VCM_t));
pcm->header.messageID = 43;
pcm->header.protocolVersion = 1;
vcm->header.messageID = 43;
vcm->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;
vcm->header.stationID = facilities->id.station_id;
pthread_mutex_unlock(&facilities->id.lock);
int32_t lat, lon;
@ -565,26 +565,26 @@ static int mk_pcm(facilities_t* facilities, uint8_t* pcm_uper, uint16_t* pcm_upe
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);
vcm->vcm.currentPosition.latitude = lat;
vcm->vcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&vcm->vcm.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;
vcm->vcm.chain = calloc(1, sizeof(ChainInformation_t));
vcm->vcm.chain->id = coordination->chain.id;
vcm->vcm.chain->link.buf = malloc(8);
vcm->vcm.chain->link.size = 8;
memcpy(vcm->vcm.chain->link.buf, coordination->chain.link, 8);
vcm->vcm.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;
vcm->vcm.maneuverContainer.present = ManeuverContainer_PR_rsu;
ManeuverRSUContainer_t* mrc = &vcm->vcm.maneuverContainer.choice.rsu;
mrc->recommendedTrajectories = NULL; // TODO
} else {
pcm->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &pcm->pcm.maneuverContainer.choice.vehicle;
vcm->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
// Vehicle Dimensions
mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
@ -601,37 +601,37 @@ static int mk_pcm(facilities_t* facilities, uint8_t* pcm_uper, uint16_t* pcm_upe
}
}
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_PCM, NULL, pcm, pcm_uper, 512);
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm, vcm_uper, 512);
if (enc.encoded == -1) {
syslog_err("[facilities] [pc] failed encoding PCM (%s)", enc.failed_type->name);
syslog_err("[facilities] [vc] failed encoding VCM (%s)", enc.failed_type->name);
rv = 1;
goto cleanup;
}
*pcm_uper_len = (enc.encoded + 7) / 8;
*vcm_uper_len = (enc.encoded + 7) / 8;
cleanup:
ASN_STRUCT_FREE(asn_DEF_PCM, pcm);
ASN_STRUCT_FREE(asn_DEF_VCM, vcm);
return rv;
}
static bool pcm_timer_check(coordination_s* coordination, uint64_t now) {
static bool vcm_timer_check(coordination_s* coordination, uint64_t now) {
bool send = false;
if (now > coordination->t_last_send_pcm + coordination->pcm_period_max) {
if (now > coordination->t_last_send_vcm + coordination->vcm_period_max) {
send = true;
}
return send;
}
void* pc_service(void* fc) {
void* vc_service(void* fc) {
facilities_t* facilities = (facilities_t*) fc;
coordination_s* coordination = (coordination_s*) &facilities->coordination;
uint8_t pcm[512];
uint8_t vcm[512];
TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t));
tr->present = TransportRequest_PR_packet;
@ -664,8 +664,8 @@ void* pc_service(void* fc) {
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 (vcm_timer_check(coordination, now)) {
rv = mk_vcm(facilities, bpr->data.buf, (uint16_t *) &bpr->data.size);
if (rv) {
continue;
}
@ -674,7 +674,7 @@ void* pc_service(void* fc) {
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");
syslog_err("[facilities] encoding TR for VCM failed");
continue;
}
it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp");
@ -684,7 +684,7 @@ void* pc_service(void* fc) {
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");
syslog_err("[facilities] encoding FI for VCM failed");
continue;
}
it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");
@ -706,7 +706,7 @@ void* pc_service(void* fc) {
}
}
coordination->t_last_send_pcm = now;
coordination->t_last_send_vcm = now;
}
pthread_mutex_unlock(&coordination->lock);

View File

@ -1,7 +1,7 @@
#pragma once
#include <stdbool.h>
#include <pcm/PCM.h>
#include <vcm/VCM.h>
#include <it2s-tender/geodesy.h>
@ -23,9 +23,9 @@ typedef struct coordination {
pthread_mutex_t lock;
uint64_t t_last_send_pcm;
uint64_t pcm_period_min;
uint64_t pcm_period_max;
uint64_t t_last_send_vcm;
uint64_t vcm_period_min;
uint64_t vcm_period_max;
mc_neighbour_s neighbours[MC_MAX_NEIGHBOURS];
uint8_t neighbours_len;
@ -38,6 +38,6 @@ typedef struct coordination {
} coordination_s;
int pcm_check(void* fc, PCM_t* pcm);
void* pc_service(void* fc);
int vcm_check(void* fc, VCM_t* vcm);
void* vc_service(void* fc);
void coordination_init(coordination_s* coordination);