RSU, OBU CAM stuff

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

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;
+    struct timespec systemtime;
-    if (clock_gettime(CLOCK_REALTIME, &st)) {
+    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));
+        if (!isnan(gps_data.gps_track)) {
-    cam_tx->cam.camParameters.lowFrequencyContainer->present = LowFrequencyContainer_PR_basicVehicleContainerLowFrequency;
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = ((uint32_t)(gps_data.gps_track * 10));
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.vehicleRole = VehicleRole_default;
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = getHeadingConfidence((uint32_t)(gps_data.gps_epd * 10));
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf = calloc(1, sizeof(uint8_t) * 1);
+        } else {
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf[0] = 1 << 6;
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = HeadingValue_unavailable;
-    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.bits_unused = 5;
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = HeadingConfidence_unavailable;
-    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;
         
-    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 (!isnan(gps_data.gps_speed)) {
-    if (clock_gettime(CLOCK_REALTIME, &systemtime)) {
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = ((uint32_t)(gps_data.gps_speed * 100));  // cm/s
-        syslog_emerg("clock_gettime() failed");
+            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);

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

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

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

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;