TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

PCM->VCM

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/CMakeLists.txt
View File

@ -9,7 +9,7 @@ ADD_EXECUTABLE(it2s-itss-facilities
cpm.c cpm.c
saem.c saem.c
tpm.c tpm.c
pcm.c vcm.c
) )
TARGET_LINK_LIBRARIES(it2s-itss-facilities TARGET_LINK_LIBRARIES(it2s-itss-facilities
@ -27,7 +27,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
-lit2s-asn-cpm -lit2s-asn-cpm
-lit2s-asn-saem -lit2s-asn-saem
-lit2s-asn-tpm -lit2s-asn-tpm
-lit2s-asn-pcm -lit2s-asn-vcm
-lit2s-tender -lit2s-tender
-lm -lm
-lrt -lrt

6
src/config.c
View File

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

10
src/facilities.c
View File

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

4
src/facilities.h
View File

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

218
src/pcm.c → src/vcm.c
View File

@ -1,4 +1,4 @@
#include "pcm.h" #include "vcm.h"
#include "facilities.h" #include "facilities.h"
#include <it2s-tender/epv.h> #include <it2s-tender/epv.h>
@ -8,7 +8,7 @@
#include <it2s-tender/recorder.h> #include <it2s-tender/recorder.h>
#include <itss-transport/TransportRequest.h> #include <itss-transport/TransportRequest.h>
#include <itss-facilities/FacilitiesIndication.h> #include <itss-facilities/FacilitiesIndication.h>
#include <pcm/PCM.h> #include <vcm/VCM.h>
static int are_vehicles_intersecting( 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* tA, int tA_len, uint16_t vA_length, uint16_t vA_width,
@ -48,14 +48,14 @@ static int are_vehicles_intersecting(
return 0; 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; int rv = 0;
coordination_s* coordination = &facilities->coordination; 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; TransportRequest_t* tr = NULL;
FacilitiesIndication_t* fi = 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[0].timestamp = now;
++trajectoryA_len; ++trajectoryA_len;
pcm_rep = calloc(1, sizeof(PCM_t)); vcm_rep = calloc(1, sizeof(VCM_t));
pcm_rep->header.messageID = 43; vcm_rep->header.messageID = 43;
pcm_rep->header.protocolVersion = 1; vcm_rep->header.protocolVersion = 1;
pcm_rep->header.stationID = 1; vcm_rep->header.stationID = 1;
pthread_mutex_lock(&facilities->id.lock); 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); pthread_mutex_unlock(&facilities->id.lock);
pcm_rep->pcm.currentPosition.latitude = lat; vcm_rep->vcm.currentPosition.latitude = lat;
pcm_rep->pcm.currentPosition.longitude = lon; vcm_rep->vcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&pcm_rep->pcm.currentPosition.timestamp, now); asn_ulong2INTEGER(&vcm_rep->vcm.currentPosition.timestamp, now);
if (coordination->chain.id) { if (coordination->chain.id) {
pcm_rep->pcm.chain = calloc(1, sizeof(ChainInformation_t)); vcm_rep->vcm.chain = calloc(1, sizeof(ChainInformation_t));
pcm_rep->pcm.chain->id = coordination->chain.id; vcm_rep->vcm.chain->id = coordination->chain.id;
pcm_rep->pcm.chain->link.buf = malloc(8); vcm_rep->vcm.chain->link.buf = malloc(8);
pcm_rep->pcm.chain->link.size = 8; vcm_rep->vcm.chain->link.size = 8;
memcpy(pcm_rep->pcm.chain->link.buf, coordination->chain.link, 8); memcpy(vcm_rep->vcm.chain->link.buf, coordination->chain.link, 8);
pcm_rep->pcm.chain->area.present = ChainGeoRegion_PR_circle; vcm_rep->vcm.chain->area.present = ChainGeoRegion_PR_circle;
} }
pcm_rep->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &pcm_rep->pcm.maneuverContainer.choice.vehicle; ManeuverVehicleContainer_t* mvc = &vcm_rep->vcm.maneuverContainer.choice.vehicle;
// Vehicle DimensionsDimensions // Vehicle DimensionsDimensions
mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length; 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 // Accepted trajectory
if (!request->desiredTrajectories.list.count) { 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; rv = 1;
goto cleanup; 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 = malloc(1*sizeof(void*));
mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = malloc(sizeof(long)); 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.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) { 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; rv = 1;
goto cleanup; 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 = calloc(1, sizeof(TransportRequest_t));
tr->present = TransportRequest_PR_packet; 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->gn.packetTransportType = PacketTransportType_shb;
bpr->destinationPort = 2043; bpr->destinationPort = 2043;
bpr->gn.trafficClass = 2; bpr->gn.trafficClass = 2;
bpr->data.buf = malloc(pcm_rep_len); bpr->data.buf = malloc(vcm_rep_len);
memcpy(bpr->data.buf, buf, pcm_rep_len); memcpy(bpr->data.buf, buf, vcm_rep_len);
bpr->data.size = pcm_rep_len; bpr->data.size = vcm_rep_len;
buf[0] = 4; buf[0] = 4;
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1); enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1);
if (enc.encoded == -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; rv = 1;
goto cleanup; goto cleanup;
} }
@ -179,7 +179,7 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei
buf[0] = 4; buf[0] = 4;
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, buf_len-1); enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, buf_len-1);
if (enc.encoded == -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; rv = 1;
goto cleanup; goto cleanup;
} }
@ -204,25 +204,25 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei
} }
cleanup: 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_TransportRequest, tr);
ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi); ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);
return rv; 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; 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); pthread_mutex_lock(&facilities->epv.time.lock);
uint64_t now_us = it2s_tender_get_now(TIME_MICROSECONDS); uint64_t now_us = it2s_tender_get_now(TIME_MICROSECONDS);
pthread_mutex_unlock(&facilities->epv.time.lock); pthread_mutex_unlock(&facilities->epv.time.lock);
if (neighbour->intersecting) { 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 { } 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; 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; int rv = 0;
coordination_s* coordination = &facilities->coordination; coordination_s* coordination = &facilities->coordination;
PCM_t* pcm_req = NULL; VCM_t* vcm_req = NULL;
TransportRequest_t* tr = NULL; TransportRequest_t* tr = NULL;
FacilitiesIndication_t* fi = NULL; FacilitiesIndication_t* fi = NULL;
@ -286,30 +286,30 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm,
++trajectoryA_len; ++trajectoryA_len;
// Initiate conflict resolution // Initiate conflict resolution
pcm_req = calloc(1, sizeof(PCM_t)); vcm_req = calloc(1, sizeof(VCM_t));
pcm_req->header.messageID = 43; vcm_req->header.messageID = 43;
pcm_req->header.protocolVersion = 1; vcm_req->header.protocolVersion = 1;
pcm_req->header.stationID = 1; vcm_req->header.stationID = 1;
pthread_mutex_lock(&facilities->id.lock); 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); pthread_mutex_unlock(&facilities->id.lock);
pcm_req->pcm.currentPosition.latitude = lat; vcm_req->vcm.currentPosition.latitude = lat;
pcm_req->pcm.currentPosition.longitude = lon; vcm_req->vcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&pcm_req->pcm.currentPosition.timestamp, now); asn_ulong2INTEGER(&vcm_req->vcm.currentPosition.timestamp, now);
if (coordination->chain.id) { if (coordination->chain.id) {
pcm_req->pcm.chain = calloc(1, sizeof(ChainInformation_t)); vcm_req->vcm.chain = calloc(1, sizeof(ChainInformation_t));
pcm_req->pcm.chain->id = coordination->chain.id; vcm_req->vcm.chain->id = coordination->chain.id;
pcm_req->pcm.chain->link.buf = malloc(8); vcm_req->vcm.chain->link.buf = malloc(8);
pcm_req->pcm.chain->link.size = 8; vcm_req->vcm.chain->link.size = 8;
memcpy(pcm_req->pcm.chain->link.buf, coordination->chain.link, 8); memcpy(vcm_req->vcm.chain->link.buf, coordination->chain.link, 8);
pcm_req->pcm.chain->area.present = ChainGeoRegion_PR_circle; vcm_req->vcm.chain->area.present = ChainGeoRegion_PR_circle;
} }
pcm_req->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; vcm_req->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &pcm_req->pcm.maneuverContainer.choice.vehicle; ManeuverVehicleContainer_t* mvc = &vcm_req->vcm.maneuverContainer.choice.vehicle;
// Vehicle Dimensions // Vehicle Dimensions
mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length; 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 = malloc(1*sizeof(void*));
mvc->negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t)); mvc->negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t));
ProposedTrajectory_t* pt = mvc->negotiation->choice.request.desiredTrajectories.list.array[0]; 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.count = trajectoryA_len;
pt->trajectory.list.size = sizeof(void*) * 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->priority = 1;
pt->id = 0; 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) { 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; rv = 1;
goto cleanup; 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 = calloc(1, sizeof(TransportRequest_t));
tr->present = TransportRequest_PR_packet; 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->gn.packetTransportType = PacketTransportType_shb;
bpr->destinationPort = 2043; bpr->destinationPort = 2043;
bpr->gn.trafficClass = 2; bpr->gn.trafficClass = 2;
bpr->data.buf = malloc(pcm_req_len); bpr->data.buf = malloc(vcm_req_len);
memcpy(bpr->data.buf, buf, pcm_req_len); memcpy(bpr->data.buf, buf, vcm_req_len);
bpr->data.size = pcm_req_len; bpr->data.size = vcm_req_len;
buf[0] = 4; buf[0] = 4;
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1); enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1);
if (enc.encoded == -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; rv = 1;
goto cleanup; goto cleanup;
} }
@ -396,7 +396,7 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm,
buf[0] = 4; buf[0] = 4;
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, buf_len-1); enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, buf_len-1);
if (enc.encoded == -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; rv = 1;
goto cleanup; goto cleanup;
} }
@ -421,13 +421,13 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm,
} }
cleanup: 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_TransportRequest, tr);
ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi); ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);
return rv; return rv;
} }
int pcm_check(void* fc, PCM_t* pcm) { int vcm_check(void* fc, VCM_t* vcm) {
facilities_t* facilities = (facilities_t*) fc; facilities_t* facilities = (facilities_t*) fc;
coordination_s* coordination = &facilities->coordination; coordination_s* coordination = &facilities->coordination;
@ -452,7 +452,7 @@ int pcm_check(void* fc, PCM_t* pcm) {
// Find neighbours // Find neighbours
int ni = -1; int ni = -1;
for (int i = 0; i < coordination->neighbours_len; ++i) { 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; ni = i;
break; break;
} }
@ -460,22 +460,22 @@ int pcm_check(void* fc, PCM_t* pcm) {
// Add it if not found // Add it if not found
if (ni == -1 && coordination->neighbours_len + 1 < MC_MAX_NEIGHBOURS) { 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; ni = coordination->neighbours_len;
++coordination->neighbours_len; ++coordination->neighbours_len;
} }
switch (pcm->pcm.maneuverContainer.present) { switch (vcm->vcm.maneuverContainer.present) {
case ManeuverContainer_PR_vehicle: case ManeuverContainer_PR_vehicle:
; ;
ManeuverVehicleContainer_t* mvc = &pcm->pcm.maneuverContainer.choice.vehicle; ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
if (mvc->negotiation) { if (mvc->negotiation) {
switch (mvc->negotiation->present) { switch (mvc->negotiation->present) {
case CoordinationNegotiation_PR_request: case CoordinationNegotiation_PR_request:
pcm_check_handle_request(facilities, pcm, &coordination->neighbours[ni]); vcm_check_handle_request(facilities, vcm, &coordination->neighbours[ni]);
break; break;
case CoordinationNegotiation_PR_reply: case CoordinationNegotiation_PR_reply:
pcm_check_handle_reply(facilities, pcm, &coordination->neighbours[ni]); vcm_check_handle_reply(facilities, vcm, &coordination->neighbours[ni]);
break; break;
default: default:
break; break;
@ -492,9 +492,9 @@ int pcm_check(void* fc, PCM_t* pcm) {
trajectoryA[0].timestamp = now; trajectoryA[0].timestamp = now;
++trajectoryA_len; ++trajectoryA_len;
trajectoryB[0].latitude = pcm->pcm.currentPosition.latitude; trajectoryB[0].latitude = vcm->vcm.currentPosition.latitude;
trajectoryB[0].longitude = pcm->pcm.currentPosition.longitude; trajectoryB[0].longitude = vcm->vcm.currentPosition.longitude;
asn_INTEGER2ulong(&pcm->pcm.currentPosition.timestamp, (unsigned long long*) &trajectoryB[0].timestamp); asn_INTEGER2ulong(&vcm->vcm.currentPosition.timestamp, (unsigned long long*) &trajectoryB[0].timestamp);
++trajectoryB_len; ++trajectoryB_len;
for (int i = 0; i < mvc->plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) { 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].latitude = mvc->plannedTrajectory.list.array[i]->latitude;
@ -514,20 +514,20 @@ int pcm_check(void* fc, PCM_t* pcm) {
} }
if (intersecting) { if (intersecting) {
syslog_info("[facilities] [pc] intersection danger with %ld @ (%f, %f) in %ld ms", syslog_info("[facilities] [vc] intersection danger with %ld @ (%f, %f) in %ld ms",
pcm->header.stationID, vcm->header.stationID,
trajectoryA[index].latitude/1.0e7, trajectoryA[index].latitude/1.0e7,
trajectoryA[index].longitude/1.0e7, trajectoryA[index].longitude/1.0e7,
trajectoryA[index].timestamp - now); trajectoryA[index].timestamp - now);
pcm_check_intersection_detected(facilities, pcm, &coordination->neighbours[ni]); vcm_check_intersection_detected(facilities, vcm, &coordination->neighbours[ni]);
} }
} }
break; break;
case ManeuverContainer_PR_rsu: case ManeuverContainer_PR_rsu:
break; break;
default: default:
syslog_debug("[facilities] [pc] received PCM contains unrecognized ManeuverContainer type"); syslog_debug("[facilities] [vc] received VCM contains unrecognized ManeuverContainer type");
rv = 1; rv = 1;
} }
@ -536,20 +536,20 @@ int pcm_check(void* fc, PCM_t* pcm) {
return rv; 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; int rv = 0;
coordination_s* coordination = &facilities->coordination; coordination_s* coordination = &facilities->coordination;
it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN]; 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; vcm->header.messageID = 43;
pcm->header.protocolVersion = 1; vcm->header.protocolVersion = 1;
uint64_t now = it2s_tender_get_clock(&facilities->epv); uint64_t now = it2s_tender_get_clock(&facilities->epv);
pthread_mutex_lock(&facilities->id.lock); 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); pthread_mutex_unlock(&facilities->id.lock);
int32_t lat, lon; 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)); memcpy(trajectory, facilities->epv.trajectory.path, trajectory_len * sizeof(it2s_tender_st_s));
it2s_tender_unlock_trajectory(&facilities->epv); it2s_tender_unlock_trajectory(&facilities->epv);
pcm->pcm.currentPosition.latitude = lat; vcm->vcm.currentPosition.latitude = lat;
pcm->pcm.currentPosition.longitude = lon; vcm->vcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&pcm->pcm.currentPosition.timestamp, now); asn_ulong2INTEGER(&vcm->vcm.currentPosition.timestamp, now);
if (coordination->chain.id) { if (coordination->chain.id) {
pcm->pcm.chain = calloc(1, sizeof(ChainInformation_t)); vcm->vcm.chain = calloc(1, sizeof(ChainInformation_t));
pcm->pcm.chain->id = coordination->chain.id; vcm->vcm.chain->id = coordination->chain.id;
pcm->pcm.chain->link.buf = malloc(8); vcm->vcm.chain->link.buf = malloc(8);
pcm->pcm.chain->link.size = 8; vcm->vcm.chain->link.size = 8;
memcpy(pcm->pcm.chain->link.buf, coordination->chain.link, 8); memcpy(vcm->vcm.chain->link.buf, coordination->chain.link, 8);
pcm->pcm.chain->area.present = ChainGeoRegion_PR_circle; vcm->vcm.chain->area.present = ChainGeoRegion_PR_circle;
} }
if (facilities->station_type == StationType_roadSideUnit) { if (facilities->station_type == StationType_roadSideUnit) {
pcm->pcm.maneuverContainer.present = ManeuverContainer_PR_rsu; vcm->vcm.maneuverContainer.present = ManeuverContainer_PR_rsu;
ManeuverRSUContainer_t* mrc = &pcm->pcm.maneuverContainer.choice.rsu; ManeuverRSUContainer_t* mrc = &vcm->vcm.maneuverContainer.choice.rsu;
mrc->recommendedTrajectories = NULL; // TODO mrc->recommendedTrajectories = NULL; // TODO
} else { } else {
pcm->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; vcm->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &pcm->pcm.maneuverContainer.choice.vehicle; ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
// Vehicle Dimensions // Vehicle Dimensions
mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length; mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0; 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) { 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; rv = 1;
goto cleanup; goto cleanup;
} }
*pcm_uper_len = (enc.encoded + 7) / 8; *vcm_uper_len = (enc.encoded + 7) / 8;
cleanup: cleanup:
ASN_STRUCT_FREE(asn_DEF_PCM, pcm); ASN_STRUCT_FREE(asn_DEF_VCM, vcm);
return rv; 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; 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; send = true;
} }
return send; return send;
} }
void* pc_service(void* fc) { void* vc_service(void* fc) {
facilities_t* facilities = (facilities_t*) fc; facilities_t* facilities = (facilities_t*) fc;
coordination_s* coordination = (coordination_s*) &facilities->coordination; coordination_s* coordination = (coordination_s*) &facilities->coordination;
uint8_t pcm[512]; uint8_t vcm[512];
TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t)); TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t));
tr->present = TransportRequest_PR_packet; tr->present = TransportRequest_PR_packet;
@ -664,8 +664,8 @@ void* pc_service(void* fc) {
uint64_t now = it2s_tender_get_clock(&facilities->epv); uint64_t now = it2s_tender_get_clock(&facilities->epv);
pthread_mutex_lock(&coordination->lock); pthread_mutex_lock(&coordination->lock);
if (pcm_timer_check(coordination, now)) { if (vcm_timer_check(coordination, now)) {
rv = mk_pcm(facilities, bpr->data.buf, (uint16_t *) &bpr->data.size); rv = mk_vcm(facilities, bpr->data.buf, (uint16_t *) &bpr->data.size);
if (rv) { if (rv) {
continue; 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); asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, buf+1, 1023);
if (enc.encoded == -1) { if (enc.encoded == -1) {
syslog_err("[facilities] encoding TR for PCM failed"); syslog_err("[facilities] encoding TR for VCM failed");
continue; continue;
} }
it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp"); 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); 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); enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, 1023);
if (enc.encoded == -1) { if (enc.encoded == -1) {
syslog_err("[facilities] encoding FI for PCM failed"); syslog_err("[facilities] encoding FI for VCM failed");
continue; continue;
} }
it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message"); 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); pthread_mutex_unlock(&coordination->lock);

12
src/pcm.h → src/vcm.h
View File

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