RSU, OBU CAM stuff

2020-10-30 19:02:35 +00:00 · 2020-10-30 19:02:35 +00:00 · 7caf37b3f3
parent 4f2a6e6fea
commit 7caf37b3f3
6 changed files with 198 additions and 89 deletions
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@ -10,9 +10,9 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
    -lit2s-asn-itss-facilities
    -lit2s-asn-itss-transport
    -lzmq
-    -ltoml
    -lit2s-gps
    -lpthread
+    -ltoml
    -lit2s-asn-camv2
    -lit2s-asn-ivim
    -lit2s-asn-denmv2
--- a/src/cam.c
+++ b/src/cam.c
@ -2,6 +2,7 @@
 #include "facilities.h"

 #include <itss-transport/BTPDataRequest.h>
+#include <itss-transport/BTPDataIndication.h>
 #include <camv2/CAM.h>
 #include <it2s-gps.h>

@ -55,20 +56,26 @@ static SpeedConfidence_t getSpeedConfidence(uint32_t conf) {
 }


-static int mk_cam(uint8_t *cam, uint32_t *cam_len) {
+static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
    int rv = 0;

-    struct timespec st;
-    if (clock_gettime(CLOCK_REALTIME, &st)) {
+    struct timespec systemtime;
+    if (clock_gettime(CLOCK_REALTIME, &systemtime)) {
        syslog_emerg("clock_gettime() failed");
    }

+    struct it2s_gps_data gps_data;
+    it2s_gps_read(&gps_data);
+
    CAM_t *cam_tx = calloc(1, sizeof(CAM_t));
    
    cam_tx->header.protocolVersion = 2;
    cam_tx->header.messageID = ItsPduHeader__messageID_cam;
    cam_tx->header.stationID = 0;
-    cam_tx->cam.camParameters.basicContainer.stationType = StationType_passengerCar;
+    cam_tx->cam.camParameters.basicContainer.stationType = facilities->station_type;
+
+    if (facilities->station_type != StationType_roadSideUnit) {
+        cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleWidth = 20;
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthValue = 46;
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;
@ -81,30 +88,24 @@ static int mk_cam(uint8_t *cam, uint32_t *cam_len) {
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateValue = YawRateValue_unavailable;
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateConfidence = YawRateConfidence_unavailable;
    
-    cam_tx->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(struct LowFrequencyContainer));
-    cam_tx->cam.camParameters.lowFrequencyContainer->present = LowFrequencyContainer_PR_basicVehicleContainerLowFrequency;
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.vehicleRole = VehicleRole_default;
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf = calloc(1, sizeof(uint8_t) * 1);
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf[0] = 1 << 6;
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.bits_unused = 5;
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.size = 1;
-    *(cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf) = 0x00;
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.count = 0;
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.size = 0;
+        if (!isnan(gps_data.gps_track)) {
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = ((uint32_t)(gps_data.gps_track * 10));
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = getHeadingConfidence((uint32_t)(gps_data.gps_epd * 10));
+        } else {
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = HeadingValue_unavailable;
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = HeadingConfidence_unavailable;
        
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.count = 1;
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.size = 1;
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array = calloc(1, sizeof(struct PathPoint*));
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0] = calloc(1, sizeof(struct PathPoint));
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathDeltaTime = calloc(1, sizeof(PathDeltaTime_t));
-    *(cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathDeltaTime) = PathDeltaTime_tenMilliSecondsInPast;
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;
+        }

-    struct timespec systemtime;
-    if (clock_gettime(CLOCK_REALTIME, &systemtime)) {
-        syslog_emerg("clock_gettime() failed");
+        if (!isnan(gps_data.gps_speed)) {
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = ((uint32_t)(gps_data.gps_speed * 100));  // cm/s
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = getSpeedConfidence((uint32_t)(gps_data.gps_eps * 100));
+        } else {
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = SpeedValue_unavailable;
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = SpeedConfidence_unavailable;
+        }
+    } else {
+        cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency;
    }

    long generationdeltatime = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
@ -112,8 +113,6 @@ static int mk_cam(uint8_t *cam, uint32_t *cam_len) {
    generationdeltatime = generationdeltatime % 64536;
    cam_tx->cam.generationDeltaTime = generationdeltatime;

-    struct it2s_gps_data gps_data;
-    it2s_gps_read(&gps_data);
   if (!isnan(gps_data.gps_altitude)) {
       cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = (int32_t)((gps_data.gps_altitude) * 10);
       cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = getAltitudeConfidence(gps_data.gps_epv);
@ -138,27 +137,9 @@ static int mk_cam(uint8_t *cam, uint32_t *cam_len) {
    cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
    cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
    
-    cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
-    
-    if (!isnan(gps_data.gps_track)) {
-        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = ((uint32_t)(gps_data.gps_track * 10));
-        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = getHeadingConfidence((uint32_t)(gps_data.gps_epd * 10));
-    } else {
-        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = HeadingValue_unavailable;
-        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = HeadingConfidence_unavailable;
-    
-    }
-
-    if (!isnan(gps_data.gps_speed)) {
-        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = ((uint32_t)(gps_data.gps_speed * 100));  // cm/s
-        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = getSpeedConfidence((uint32_t)(gps_data.gps_eps * 100));
-    } else {
-        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = SpeedValue_unavailable;
-        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = SpeedConfidence_unavailable;
-    }
-
    asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_CAM, NULL, cam_tx, cam, 256);
    if (enc.encoded == -1) {
+        syslog_err("[facilities] [ca] failed encoding CAM (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup; 
    }
@ -171,6 +152,82 @@ cleanup:
 }


+lightship_t* lightship_init() {
+    lightship_t* lightship = (lightship_t*) calloc(1, sizeof(lightship_t));
+
+    pthread_mutex_init(&lightship->lock, NULL);
+
+    return lightship;
+}
+
+
+int lightship_check(lightship_t* lightship) {
+    int rv = 0;
+
+    struct timespec systemtime;
+    clock_gettime(CLOCK_REALTIME, &systemtime);
+    long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
+    now -= 1072915200000;
+
+    pthread_mutex_lock(&lightship->lock);
+
+    if (lightship->type == StationType_roadSideUnit) { // RSU
+        if (lightship->is_vehicle_near && now > lightship->next_cam) {
+            rv = 1; 
+        }
+    } else { // Vehicle
+        if (now > lightship->next_cam) {
+            rv = 1;
+        }
+    }
+
+
+    pthread_mutex_unlock(&lightship->lock);
+    
+    return rv;
+}
+
+void lightship_reset_timer(lightship_t* lightship) {
+    struct timespec systemtime;
+    clock_gettime(CLOCK_REALTIME, &systemtime);
+    long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
+    now -= 1072915200000;
+
+    pthread_mutex_lock(&lightship->lock);
+
+    if (lightship->type != StationType_roadSideUnit) { // Vehicle
+        lightship->next_cam = now + VEHICLE_GEN_CAM_MAX;
+    } else { // RSU
+        if (now > lightship->last_vehicle + RSU_VEHICLE_PERMANENCE) {
+            lightship->is_vehicle_near = false;
+        }
+        lightship->next_cam = now + RSU_GEN_CAM_MIN;
+    }
+
+    pthread_mutex_unlock(&lightship->lock);
+}
+
+int check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam) {
+    int rv = 0;
+    facilities_t *facilities = (facilities_t*) fc;
+
+    struct timespec systemtime;
+    clock_gettime(CLOCK_REALTIME, &systemtime);
+    long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
+    now -= 1072915200000;
+
+    pthread_mutex_lock(&facilities->lightship->lock);
+    if (facilities->lightship->type == StationType_roadSideUnit) {
+        if (bdi->stationType != StationType_roadSideUnit && bdi->isNeighbour) {
+            facilities->lightship->last_vehicle = now;
+            facilities->lightship->is_vehicle_near = true;
+        }
+    }
+    pthread_mutex_unlock(&facilities->lightship->lock);
+
+    return rv;
+}
+
 void *ca_service(void *fc) {

    int rv = 0;
@ -199,9 +256,11 @@ void *ca_service(void *fc) {
    uint8_t bdr_oer[256];
    bdr_oer[0] = 4; // Facilities
    while (!facilities->exit) {
-        rv = mk_cam(bdr->data.buf, (uint32_t *) &bdr->data.size);
+        usleep(1000*50);
+
+        if ( lightship_check(facilities->lightship) ) {
+            rv = mk_cam(facilities, bdr->data.buf, (uint32_t *) &bdr->data.size);
            if (rv) {
-            syslog_err("[facilities] make cam failed (%d)", rv);
                continue;
            }

@ -214,7 +273,9 @@ void *ca_service(void *fc) {
            queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3);
            pthread_cond_signal(&facilities->tx_queue->trigger);

-        usleep(1000*1000);
+            lightship_reset_timer(facilities->lightship);
+        }
+
    }

    ASN_STRUCT_FREE(asn_DEF_BTPDataRequest, bdr);
--- a/src/cam.h
+++ b/src/cam.h
@ -2,7 +2,37 @@
 #define FACILITIES_CAM_H

 #include <stdint.h>
+#include <stdlib.h>
+#include <pthread.h>
+#include <stdbool.h>

-void *ca_service(void *fc);
+#include <camv2/CAM.h>
+#include <itss-transport/BTPDataIndication.h>
+
+#define VEHICLE_GEN_CAM_MIN 100
+#define VEHICLE_GEN_CAM_MAX 1000
+
+#define RSU_GEN_CAM_MIN 1000
+#define RSU_VEHICLE_PERMANENCE 5000
+
+typedef struct lightship {
+    pthread_mutex_t lock;
+
+    uint8_t type;
+
+    uint64_t next_cam;
+
+    bool is_vehicle_near;
+    uint64_t last_vehicle;
+
+} lightship_t;
+
+lightship_t* lightship_init();
+
+int lightship_check(lightship_t *lightship);
+void lightship_reset_timer(lightship_t *lightship);
+
+int check_cam(void* fc, BTPDataIndication_t* bdi, CAM_t* cam);
+void* ca_service(void* fc);

 #endif
--- a/src/config.h
+++ b/src/config.h
@ -1,6 +1,6 @@
 #ifndef ITSS_CONFIG_H
 #define ITSS_CONFIG_H

-int itss_config(void *itss_s, char * config_file);
+int itss_config(void *facilities_s, char * config_file);

 #endif
--- a/src/facilities.c
+++ b/src/facilities.c
@ -77,18 +77,29 @@ static int transport_indication(facilities_t *facilities, void* responder, uint8
    }

    // Manage message
-    if (bdi->destinationPort == Port_denm) {
+    switch (bdi->destinationPort) {
+        case Port_cam:
+
+            if (facilities->station_type == 15) { // Currently only RSUs need to check CAMs
+                check_cam(facilities, bdi, its_msg);
+            }
+
+            break;
+        case Port_denm:
            #ifdef DEBUG
                uint8_t* xml_denm = malloc(4096);
                asn_enc_rval_t rve = xer_encode_to_buffer(xml_denm, 4096, 0x02, &asn_DEF_DENM, its_msg);
                syslog_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded , xml_denm);
                free(xml_denm);
            #endif
-
            event_manage(facilities->den, its_msg);
+
+            break;
+        default:
+            break;
    }

-    // Forward to app
+    // Forward to application
    fdi = calloc(1, sizeof(FacilitiesDataIndication_t));

    fdi->itssMessageType = bdi->destinationPort;
@ -356,6 +367,9 @@ int main() {

    facilities.tx_queue = queue_init();

+    facilities.lightship = lightship_init();
+    facilities.lightship->type = facilities.station_type;
+
    facilities.den = calloc(1, sizeof(den_t));

    // Tx
--- a/src/facilities.h
+++ b/src/facilities.h
@ -4,6 +4,7 @@
 #include <pthread.h>
 #include <stdbool.h>

+#include "cam.h"
 #include "denm.h"
 #include "queue.h"

@ -22,7 +23,10 @@ typedef struct facilities {
    // Transmitter
    queue_t* tx_queue;

-    // DENM
+    // CA
+    lightship_t* lightship;
+
+    // DEN
    den_t *den;

    int station_type;