diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 8bf5241..0a0d609 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -30,11 +30,11 @@ stages: deploy release: stage: deploy release script: - - curl -fs http://192.168.94.221:3000/arch/it2s-itss-facilities-git + - curl -fs http://192.168.94.221:3000/arch/it2s-itss-vfacilities-git # - curl -fs http://192.168.94.221:3000/ubuntu_arm64/it2s-itss-facilities-git deploy debug: stage: deploy debug script: - - curl -fs http://192.168.94.221:3000/arch/it2s-itss-facilities-debug-git + - curl -fs http://192.168.94.221:3000/arch/it2s-itss-vfacilities-debug-git # - curl -fs http://192.168.94.221:3000/ubuntu_arm64/it2s-itss-facilities-debug-git diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ca34548..be24d68 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -11,6 +11,7 @@ ADD_EXECUTABLE(it2s-itss-facilities tpm.c vcm.c evm.c + edm.c ) TARGET_LINK_LIBRARIES(it2s-itss-facilities @@ -31,7 +32,6 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities -lit2s-asn-vcm -lit2s-asn-evcsnm -lit2s-asn-evrsrm - -lit2s-asn-verco -lit2s-tender -lit2s-obd -lm diff --git a/src/cam.c b/src/cam.c index ee5ae9a..a4d1a32 100644 --- a/src/cam.c +++ b/src/cam.c @@ -157,7 +157,7 @@ static void path_history_update(pos_point_t* pn) { } } -static int mk_cam(uint8_t *cam_oer, uint32_t *cam_len) { +static int mk_cam(uint8_t *cam_oer, uint16_t *cam_len) { int rv = 0; int shm_fd, shm_valid = 0; it2s_obd_data* shared_message; @@ -824,7 +824,16 @@ void* ca_service() { bpr->gn.trafficClass = 2; - bpr->data.buf = malloc(512); + if (facilities.edm.enabled) { + bpr->destinationPortInfo = calloc(1, sizeof(OCTET_STRING_t)); + bpr->destinationPortInfo->size = 2; + bpr->destinationPortInfo->buf = malloc(2); + *(uint16_t*)bpr->destinationPortInfo->buf = 0xed; + } + + const int buf_len = 1024; + + bpr->data.buf = malloc(buf_len); // Fill header for FacilitiesIndication and FacilitiesMessageIndication structs @@ -834,20 +843,25 @@ void* ca_service() { fi->present = FacilitiesIndication_PR_message; FacilitiesMessageIndication_t* fmi = &fi->choice.message; fmi->itsMessageType = ItsMessageType_cam; - fmi->data.buf = malloc(512); + fmi->data.buf = malloc(buf_len); - uint8_t tr_oer[1024]; - uint8_t fi_oer[1024]; + uint8_t tr_oer[buf_len]; + uint8_t fi_oer[buf_len]; tr_oer[0] = 4; // Facilities fi_oer[0] = 4; while (!facilities.exit) { usleep(1000*50); if (lightship_check() && facilities.lightship.active) { - rv = mk_cam(bpr->data.buf, (uint32_t *) &bpr->data.size); + rv = mk_cam(bpr->data.buf, (uint16_t*) &bpr->data.size); if (rv) { continue; } + + if (facilities.edm.enabled) { + edm_encap(bpr->data.buf, (uint16_t*) &bpr->data.size, buf_len, Port_cam); + } + memcpy(fmi->data.buf, bpr->data.buf, bpr->data.size); fmi->data.size = bpr->data.size; diff --git a/src/config.c b/src/config.c index edf0365..c438b2d 100644 --- a/src/config.c +++ b/src/config.c @@ -498,6 +498,8 @@ int facilities_config() { itss_epv_init(); + facilities.edm.enabled = config->applications.extensions.enabled; + ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t)); mreq->present = ManagementRequest_PR_attributes; mreq->choice.attributes.present = ManagementRequestAttributes_PR_get; diff --git a/src/edm.c b/src/edm.c new file mode 100644 index 0000000..8b3b9ae --- /dev/null +++ b/src/edm.c @@ -0,0 +1,71 @@ +#include "edm.h" +#include "facilities.h" +#include +#include +#include + +int edm_encap(uint8_t* msg, uint16_t* msg_len, uint16_t msg_buf_len, int its_msg_type) { + edm_t* edm = &facilities.edm; + int rv = 0; + + FacilitiesIndication_t* fi = NULL; + FacilitiesResponse_t* fr = NULL; + + uint16_t b_len = 1380; + uint8_t b1[b_len], b2[b_len]; + + fi = calloc(1, sizeof(FacilitiesIndication_t)); + fi->present = FacilitiesIndication_PR_extension; + fi->choice.extension.data.size = *msg_len; + fi->choice.extension.data.buf = malloc(*msg_len); + memcpy(fi->choice.extension.data.buf, msg, *msg_len); + fi->choice.extension.itsMessageType = its_msg_type; + + b1[0] = ITSS_FACILITIES; + asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, b1+1, b_len-1); + if (enc.encoded == -1) { + return 1; + } + + pthread_mutex_lock(&edm->lock); // TODO do not use single apps socket + itss_0send(edm->app_socket, b1, enc.encoded+1); + int rl = itss_0recv_rt(&edm->app_socket, b2, b_len, b1, enc.encoded+1, 400); + if (rl == -1) { + rv = 1; + log_error("[edm] request extension "); + pthread_mutex_unlock(&edm->lock); + goto cleanup; + } + pthread_mutex_unlock(&edm->lock); + + asn_dec_rval_t dec = asn_decode(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesResponse, (void**) &fr, b2, rl); + if (dec.code != 0) { + rv = 1; + goto cleanup; + } + + if (fr->present != FacilitiesResponse_PR_extension || + fr->choice.extension.returnCode != 0) { + rv = 1; + goto cleanup; + } + + if (fr->choice.extension.extendedMsg.size > msg_buf_len) { + rv = 1; + goto cleanup; + } + + *msg_len = fr->choice.extension.extendedMsg.size; + memcpy(msg, fr->choice.extension.extendedMsg.buf, fr->choice.extension.extendedMsg.size); + +cleanup: + ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi); + ASN_STRUCT_FREE(asn_DEF_FacilitiesResponse, fr); + + return rv; +} + +void edm_init() { + facilities.edm.app_socket = itss_0connect(facilities.zmq.applications_address, ZMQ_REQ); + pthread_mutex_init(&facilities.edm.lock, NULL); +} diff --git a/src/edm.h b/src/edm.h new file mode 100644 index 0000000..ec9a56c --- /dev/null +++ b/src/edm.h @@ -0,0 +1,15 @@ +#pragma once + +#include +#include +#include + +typedef struct edm { + bool enabled; + pthread_mutex_t lock; + void* app_socket; +} edm_t; + +int edm_encap(uint8_t* msg, uint16_t* msg_len, uint16_t msg_buf_len, int its_msg_type); + +void edm_init(); diff --git a/src/facilities.c b/src/facilities.c index 3ec5a84..54fe63f 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -497,6 +496,11 @@ int main() { if (facilities.evm_args.activate) pthread_create(&facilities.evcsn_service, NULL, evcsn_service, NULL); + // EDM + if (facilities.edm.enabled) { + edm_init(); + } + security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); uint8_t buffer[ITSS_SDU_MAX_LEN]; @@ -596,6 +600,9 @@ cleanup: pthread_join(facilities.transmitting, NULL); if (facilities.coordination.active) pthread_join(facilities.vc_service, NULL); + if (facilities.edm.enabled) { + itss_0close(facilities.edm.app_socket); + } itss_0close(security_socket); for (int i = 0; i < facilities.zmq.n_responders; ++i) { diff --git a/src/facilities.h b/src/facilities.h index 2722045..36273a3 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -13,6 +13,7 @@ #include "tpm.h" #include "vcm.h" #include "evm.h" +#include "edm.h" #include #include @@ -75,6 +76,9 @@ typedef struct facilities { // EVCSN evm_args_t evm_args; + // EDM + edm_t edm; + // Logging struct { bool recorder; diff --git a/src/requests.c b/src/requests.c index 71da2a4..6f8d743 100644 --- a/src/requests.c +++ b/src/requests.c @@ -1,12 +1,12 @@ #include "requests.h" +#include #include #include #include #include #include #include -#include #include #include #include @@ -20,6 +20,28 @@ #include "facilities.h" #include "infrastructure.h" +static void fwd_to_apps(uint8_t* msg, uint16_t msg_len, int its_msg_type, uint32_t iid) { + FacilitiesIndication_t* fi = NULL; + + fi = calloc(1, sizeof(FacilitiesIndication_t)); + fi->present = FacilitiesIndication_PR_message; + FacilitiesMessageIndication_t *fmi = &fi->choice.message; + + fmi->id = iid; + + fmi->itsMessageType = its_msg_type; + + fmi->data.size = msg_len; + fmi->data.buf = malloc(msg_len); + memcpy(fmi->data.buf, msg, msg_len); + + uint8_t buffer[ITSS_SDU_MAX_LEN]; + buffer[0] = 4; // Facilities + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, buffer + 1, ITSS_SDU_MAX_LEN - 1); + + itss_queue_send(facilities.tx_queue, buffer, enc.encoded + 1, ITSS_APPLICATIONS, iid, "FI.message"); +} + int facilities_request_result_accepted(void *responder) { int rv = 0; @@ -98,15 +120,6 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ bpr->destinationPort = Port_cpm; bpr->gn.packetTransportType = PacketTransportType_shb; bpr->gn.trafficClass = 2; - - break; - - case 2044: // VERCOe - its_msg_def = &asn_DEF_VERCOe; - its_msg = calloc(1, sizeof(VERCOe_t)); - bpr->destinationPort = 2044; - bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->gn.trafficClass = 2; break; case ItsMessageType_evrsr: @@ -605,7 +618,6 @@ cleanup: return rv; } - int transport_indication_btp(BTPPacketIndication_t* bpi, void** security_socket) { int rv = 0; @@ -667,13 +679,6 @@ int transport_indication_btp(BTPPacketIndication_t* bpi, void** security_socket) its_msg_type = 43; break; - case 2044: /* VERCOe */ - its_msg_descriptor = &asn_DEF_VERCOe; - its_msg = calloc(1, sizeof(VERCOe_t)); - its_msg_type = 44; - fwd = true; - break; - case Port_poi: /* EVCSNM */ its_msg_descriptor = &asn_DEF_EvcsnPdu; its_msg = calloc(1, sizeof(EvcsnPdu_t)); @@ -784,10 +789,11 @@ int transport_indication_btp(BTPPacketIndication_t* bpi, void** security_socket) break; case 2043: + fwd_to_apps(bpi->data.buf, bpi->data.size, bpi->destinationPort, bpi->id); + fwd = false; if (facilities.coordination.active) { vcm_check(its_msg); } - fwd = true; break; case Port_poi: @@ -812,23 +818,7 @@ int transport_indication_btp(BTPPacketIndication_t* bpi, void** security_socket) // Forward to [applications] if (fwd) { - fi = calloc(1, sizeof(FacilitiesIndication_t)); - fi->present = FacilitiesIndication_PR_message; - FacilitiesMessageIndication_t *fmi = &fi->choice.message; - - fmi->id = bpi->id; - - fmi->itsMessageType = bpi->destinationPort; - - fmi->data.size = bpi->data.size; - fmi->data.buf = malloc(bpi->data.size); - memcpy(fmi->data.buf, bpi->data.buf, bpi->data.size); - - uint8_t buffer[ITSS_SDU_MAX_LEN]; - buffer[0] = 4; // Facilities - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, buffer + 1, ITSS_SDU_MAX_LEN - 1); - - itss_queue_send(facilities.tx_queue, buffer, enc.encoded + 1, ITSS_APPLICATIONS, bpi->id, "FI.message"); + fwd_to_apps(bpi->data.buf, bpi->data.size, bpi->destinationPort, bpi->id); } // Logging diff --git a/src/vcm.c b/src/vcm.c index 0ffd3b4..865dfd8 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -12,6 +12,10 @@ #include #include +#include + +#define TRAJECTORY_C_TH 1.0 + static int do_paths_intersect( itss_st_t* tA, int tA_len, uint16_t vA_length, uint16_t vA_width, itss_st_t* tB, int tB_len, uint16_t vB_length, uint16_t vB_width, @@ -79,14 +83,26 @@ static void tx_vcm(VCM_t* vcm) { log_error("[vc] VCM.reply encode failure (%s)", enc.failed_type->name); goto cleanup; } - ssize_t vcm_rep_len = (enc.encoded + 7) / 8; + uint16_t vcm_len = (enc.encoded + 7) / 8; 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; bpr->btpType = BTPType_btpB; - bpr->id = itss_id(buf, vcm_rep_len); + + if (facilities.edm.enabled && + vcm->vcm.maneuverContainer.present == ManeuverContainer_PR_vehicle && + vcm->vcm.maneuverContainer.choice.vehicle.negotiation + ) { + edm_encap(buf, &vcm_len, buf_len, 2043); + bpr->destinationPortInfo = calloc(1, sizeof(OCTET_STRING_t)); + bpr->destinationPortInfo->size = 2; + bpr->destinationPortInfo->buf = malloc(2); + *(uint16_t*)bpr->destinationPortInfo->buf = 0xed; + } + + bpr->id = itss_id(buf, vcm_len); bpr->gn.destinationAddress.buf = malloc(6); for (int i = 0; i < 6; ++i) { bpr->gn.destinationAddress.buf[i] = 0xff; @@ -95,9 +111,9 @@ static void tx_vcm(VCM_t* vcm) { bpr->gn.packetTransportType = PacketTransportType_shb; bpr->destinationPort = 2043; bpr->gn.trafficClass = 2; - bpr->data.buf = malloc(vcm_rep_len); - memcpy(bpr->data.buf, buf, vcm_rep_len); - bpr->data.size = vcm_rep_len; + bpr->data.buf = malloc(vcm_len); + memcpy(bpr->data.buf, buf, vcm_len); + bpr->data.size = vcm_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) { @@ -183,6 +199,7 @@ static void vcm_reject(VCM_t* vcm, mc_neighbour_s* neighbour) { vcm_rep->vcm.currentPosition.longitude = lon; asn_ulong2INTEGER(&vcm_rep->vcm.currentPosition.timestamp, now); + /* if (coordination->chain.enabled && coordination->chain.id) { vcm_rep->vcm.chain = calloc(1, sizeof(ChainInformation_t)); vcm_rep->vcm.chain->id = coordination->chain.id; @@ -199,6 +216,7 @@ static void vcm_reject(VCM_t* vcm, mc_neighbour_s* neighbour) { vcm_rep->vcm.chain->area->trees.list.array[q]->size = 4; } } + */ vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle; @@ -348,6 +366,7 @@ static bool commit() { mvc->negotiation->choice.commit.qp.choice.requesterId = coordination->session.requester->station_id; } + /* if (coordination->chain.enabled && coordination->chain.id) { vcm_com->vcm.chain = calloc(1, sizeof(ChainInformation_t)); vcm_com->vcm.chain->id = coordination->chain.id; @@ -364,6 +383,7 @@ static bool commit() { vcm_com->vcm.chain->area->trees.list.array[q]->size = 4; } } + */ tx_vcm(vcm_com); ASN_STRUCT_FREE(asn_DEF_VCM, vcm_com); @@ -495,6 +515,7 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) { vcm_rep->vcm.currentPosition.longitude = lon; asn_ulong2INTEGER(&vcm_rep->vcm.currentPosition.timestamp, now); + /* if (coordination->chain.enabled && coordination->chain.id && vcm->vcm.chain) { vcm_rep->vcm.chain = calloc(1, sizeof(ChainInformation_t)); vcm_rep->vcm.chain->id = coordination->chain.id; @@ -520,6 +541,7 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) { vcm_rep->vcm.chain->area->trees.list.array[q]->size = 4; } } + */ vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle; @@ -743,6 +765,7 @@ static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) { vcm_req->vcm.currentPosition.longitude = lon; asn_ulong2INTEGER(&vcm_req->vcm.currentPosition.timestamp, now); + /* if (coordination->chain.enabled && coordination->chain.id) { vcm_req->vcm.chain = calloc(1, sizeof(ChainInformation_t)); vcm_req->vcm.chain->id = coordination->chain.id; @@ -759,6 +782,7 @@ static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) { vcm_req->vcm.chain->area->trees.list.array[q]->size = 4; } } + */ vcm_req->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; ManeuverVehicleContainer_t* mvc = &vcm_req->vcm.maneuverContainer.choice.vehicle; @@ -1052,6 +1076,7 @@ static int mk_vcm() { vcm->vcm.currentPosition.longitude = lon; asn_ulong2INTEGER(&vcm->vcm.currentPosition.timestamp, now); + /* if (coordination->chain.enabled && coordination->chain.id) { vcm->vcm.chain = calloc(1, sizeof(ChainInformation_t)); vcm->vcm.chain->id = coordination->chain.id; @@ -1068,6 +1093,7 @@ static int mk_vcm() { vcm->vcm.chain->area->trees.list.array[q]->size = 4; } } + */ if (facilities.station_type == StationType_roadSideUnit) { vcm->vcm.maneuverContainer.present = ManeuverContainer_PR_rsu; @@ -1109,7 +1135,43 @@ static bool vcm_timer_check(coordination_t* coordination, uint64_t now) { bool send = false; if (now > coordination->t_last_send_vcm + coordination->vcm_period_max) { - send = true; + return true; + } + + if (now < coordination->t_last_send_vcm + coordination->vcm_period_min) { + return false; + } + + if (facilities.station_type != StationType_roadSideUnit) { // Vehicle + + itss_trajectory_lock(); + + if (epv.trajectory.len > 2) { + + double c_sum = 0; + for (int i = 0; i < epv.trajectory.len - 2; ++i) { + double d1 = itss_haversine( + epv.trajectory.path[i+1].latitude / 1.0e7, + epv.trajectory.path[i+1].longitude / 1.0e7, + epv.trajectory.path[i].latitude / 1.0e7, + epv.trajectory.path[i].longitude / 1.0e7); + double t1 = atan2( + epv.trajectory.path[i+1].latitude - epv.trajectory.path[i].latitude , + epv.trajectory.path[i+1].longitude - epv.trajectory.path[i].longitude); + double t2 = atan2( + epv.trajectory.path[i+2].latitude - epv.trajectory.path[i].latitude , + epv.trajectory.path[i+2].longitude - epv.trajectory.path[i].longitude); + c_sum += fabs(sin(t2-t1)*d1); + + } + + if (fabs(c_sum - coordination->previous_c) > TRAJECTORY_C_TH) { + send = true; + coordination->previous_c = c_sum; + } + } + + itss_trajectory_unlock(epv); } return send; diff --git a/src/vcm.h b/src/vcm.h index 2f641fb..231f17a 100644 --- a/src/vcm.h +++ b/src/vcm.h @@ -50,6 +50,7 @@ typedef struct coordination { uint64_t t_last_send_vcm; /* timestamp of last sent (basic) VCM */ uint64_t vcm_period_min; uint64_t vcm_period_max; + double previous_c; struct { VCM_t* own_req; /* last VCM.request sent */