Merge branch 'edm'

.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

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

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 tr_oer[buf_len];
-    uint8_t fi_oer[1024];
+    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;
 

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;

src/edm.c

@@ -0,0 +1,71 @@
+#include "edm.h"
+#include "facilities.h"
+#include <it2s-asn/itss-facilities/FacilitiesIndication.h>
+#include <it2s-asn/itss-facilities/FacilitiesResponse.h>
+#include <it2s-tender/packet.h>
+
+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 <TIMEOUT>");
+        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);
+}

src/edm.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include <stdint.h>
+#include <pthread.h>
+#include <stdbool.h>
+
+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();

src/facilities.c

@@ -17,7 +17,6 @@
 #include <it2s-asn/itss-transport/TransportRequest.h>
 #include <it2s-asn/ivim/IVIM.h>
 #include <it2s-asn/saem/SAEM.h>
-#include <it2s-asn/verco/VERCOe.h>
 #include <it2s-tender/packet.h>
 #include <it2s-tender/recorder.h>
 #include <it2s-tender/space.h>
@@ -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) {

src/facilities.h

@@ -13,6 +13,7 @@
 #include "tpm.h"
 #include "vcm.h"
 #include "evm.h"
+#include "edm.h"
 
 #include <it2s-tender/epv.h>
 #include <it2s-tender/database.h>
@@ -75,6 +76,9 @@ typedef struct facilities {
     // EVCSN
     evm_args_t evm_args;
 
+    // EDM
+    edm_t edm;
+
     // Logging
     struct {
         bool recorder;

src/requests.c

@@ -1,12 +1,12 @@
 #include "requests.h"
 
+#include <it2s-asn/camv2/asn_application.h>
 #include <it2s-asn/cpm/CPM.h>
 #include <it2s-asn/itss-facilities/FacilitiesReply.h>
 #include <it2s-asn/itss-transport/TransportRequest.h>
 #include <it2s-asn/itss-facilities/FacilitiesIndication.h>
 #include <it2s-asn/itss-security/SecurityRequest.h>
 #include <it2s-asn/itss-security/SecurityReply.h>
-#include <it2s-asn/verco/VERCOe.h>
 #include <it2s-tender/packet.h>
 #include <it2s-tender/recorder.h>
 #include <it2s-tender/space.h>
@@ -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));
+        fwd_to_apps(bpi->data.buf, bpi->data.size, bpi->destinationPort, bpi->id);
-        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");
     }
 
     // Logging

src/vcm.c

@@ -12,6 +12,10 @@
 #include <it2s-asn/itss-management/ManagementRequest.h>
 #include <it2s-asn/vcm/VCM.h>
 
+#include <tgmath.h>
+
+#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);
+    bpr->data.buf = malloc(vcm_len);
-    memcpy(bpr->data.buf, buf, vcm_rep_len);
+    memcpy(bpr->data.buf, buf, vcm_len);
-    bpr->data.size = vcm_rep_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;

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 */