Protected zones

src/CMakeLists.txt

@@ -18,6 +18,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
     -lit2s-asn-camv2
     -lit2s-asn-ivim
     -lit2s-asn-denmv2
+    -lm
 )
 
 INSTALL(

src/cam.c

@@ -25,6 +25,9 @@
 
 #define LEAP_SECONDS 5
 
+#define EARTH_RADIUS 6369000
+#define RAD_PER_DEG M_PI_2/180.0
+
 static AltitudeConfidence_t getAltitudeConfidence(double conf) {
     if (conf >= 200) return AltitudeConfidence_outOfRange;
     if (conf >= 100) return AltitudeConfidence_alt_200_00;
@@ -108,6 +111,31 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
         }
     } else {
         cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency;
+        cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(ProtectedCommunicationZonesRSU_t));
+
+        cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.count = facilities->lightship->pz_len;
+        cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.size = facilities->lightship->pz_len * sizeof(void*);
+        cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.array = malloc(facilities->lightship->pz_len * sizeof(void*));
+        for (int i = 0; i < facilities->lightship->pz_len; ++i) {
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.array[i] = calloc(1, sizeof(ProtectedCommunicationZone_t));
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.array[i]->protectedZoneLatitude = facilities->lightship->pz[i]->protectedZoneLatitude;
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.array[i]->protectedZoneLongitude = facilities->lightship->pz[i]->protectedZoneLongitude;
+            cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.array[i]->protectedZoneType = facilities->lightship->pz[i]->protectedZoneType;
+
+            if (facilities->lightship->pz[i]->expiryTime) {
+                cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.array[i]->expiryTime = malloc(8);
+                *cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.array[i]->expiryTime = *facilities->lightship->pz[i]->expiryTime;
+            }
+            if (facilities->lightship->pz[i]->protectedZoneID) {
+                cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.array[i]->protectedZoneID = malloc(8);
+                *cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.array[i]->protectedZoneID = *facilities->lightship->pz[i]->protectedZoneID;
+            }
+            if (facilities->lightship->pz[i]->protectedZoneRadius) {
+                cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.array[i]->protectedZoneRadius = malloc(8);
+                *cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU->list.array[i]->protectedZoneRadius = *facilities->lightship->pz[i]->protectedZoneRadius;
+            }
+        }
+
     }
 
     long generationdeltatime = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
@@ -157,6 +185,7 @@ cleanup:
 lightship_t* lightship_init() {
     lightship_t* lightship = (lightship_t*) calloc(1, sizeof(lightship_t));
 
+    lightship->pz = malloc(256 * sizeof(void*));
     pthread_mutex_init(&lightship->lock, NULL);
 
     return lightship;
@@ -211,21 +240,51 @@ void lightship_reset_timer(lightship_t* lightship) {
 
 int check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam) {
     int rv = 0;
-    facilities_t *facilities = (facilities_t*) fc;
+    lightship_t *lightship = ((facilities_t*) fc)->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(&facilities->lightship->lock);
+    pthread_mutex_lock(&lightship->lock);
-    if (facilities->lightship->type == StationType_roadSideUnit) {
+    if (lightship->type == StationType_roadSideUnit) {
+        // Send CAMs if vehicles nearby
         if (bdi->stationType != StationType_roadSideUnit && bdi->isNeighbour) {
-            facilities->lightship->last_vehicle = now;
+            lightship->last_vehicle = now;
-            facilities->lightship->is_vehicle_near = true;
+            lightship->is_vehicle_near = true;
+        }
+    } else {
+        // Protected zones
+        if (cam->cam.camParameters.basicContainer.stationType == StationType_roadSideUnit &&
+            cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
+               ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
+            if (pzs->list.count > 0 && pzs->list.count + lightship->pz_len < 256) {
+                for (int i = lightship->pz_len; i < lightship->pz_len + pzs->list.count; ++i) {
+
+                    lightship->pz[i] = calloc(1, sizeof(ProtectedCommunicationZone_t));
+                    lightship->pz[i]->protectedZoneLatitude = pzs->list.array[i]->protectedZoneLatitude;
+                    lightship->pz[i]->protectedZoneLongitude = pzs->list.array[i]->protectedZoneLongitude;
+                    lightship->pz[i]->protectedZoneType = pzs->list.array[i]->protectedZoneType;
+
+                    if (pzs->list.array[i]->expiryTime) {
+                        lightship->pz[i]->expiryTime = malloc(8);
+                        *lightship->pz[i]->expiryTime = *pzs->list.array[i]->expiryTime;
+                    }
+                    if (pzs->list.array[i]->protectedZoneID) {
+                        lightship->pz[i]->protectedZoneID = malloc(8);
+                        *lightship->pz[i]->protectedZoneID = *pzs->list.array[i]->protectedZoneID;
+                    }
+                    if (pzs->list.array[i]->protectedZoneRadius) {
+                        lightship->pz[i]->protectedZoneRadius = malloc(8);
+                        *lightship->pz[i]->protectedZoneRadius = *pzs->list.array[i]->protectedZoneRadius;
+                    }
+                    ++lightship->pz_len;
+                }
+            }
         }
     }
-    pthread_mutex_unlock(&facilities->lightship->lock);
+    pthread_mutex_unlock(&lightship->lock);
 
     return rv;
 }
@@ -253,14 +312,15 @@ void *ca_service(void *fc) {
 
     bdr->gnTrafficClass = 2;
 
-    bdr->data.buf = malloc(256);
+    bdr->data.buf = malloc(384);
+
 
     if (facilities->use_security) {
         bdr->gnSecurityProfile = malloc(sizeof(long));
         *bdr->gnSecurityProfile = 1;
     }
 
-    uint8_t bdr_oer[256];
+    uint8_t bdr_oer[512];
     bdr_oer[0] = 4; // Facilities
     while (!facilities->exit) {
         usleep(1000*50);
@@ -271,7 +331,36 @@ void *ca_service(void *fc) {
                 continue;
             }
 
-            asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 255);
+            // Check if inside PZ
+            bdr->gnCommunicationProfile = 0;
+            struct it2s_gps_data gps_data;
+            it2s_gps_read(&gps_data);
+            if (facilities->lightship->type != 15) {
+                for (int i = 0; i < facilities->lightship->pz_len; ++i) {
+
+                    double d_lat = (gps_data.gps_latitude - (double) facilities->lightship->pz[i]->protectedZoneLatitude/10000000) * RAD_PER_DEG;
+                    double d_lon = (gps_data.gps_longitude - (double) facilities->lightship->pz[i]->protectedZoneLongitude/10000000) * RAD_PER_DEG;
+
+                    double a = pow(sin(d_lat/2.0), 2) + 
+                               cos(gps_data.gps_latitude * RAD_PER_DEG) * 
+                               cos((double) facilities->lightship->pz[i]->protectedZoneLatitude/10000000 * RAD_PER_DEG) * 
+                               pow(sin(d_lon/2.0), 2);
+
+                    double c = 2 * atan2(sqrt(a), sqrt(1-a));
+                    double d = EARTH_RADIUS * c;
+                  
+                    int pz_radius = 50;
+                    if (facilities->lightship->pz[i]->protectedZoneRadius) {
+                        pz_radius = *facilities->lightship->pz[i]->protectedZoneRadius;
+                    }
+                    if (d < pz_radius) {
+                        bdr->gnCommunicationProfile = 1;
+                        break;
+                    }
+                }
+            }
+
+            asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 511);
             if (enc.encoded == -1) {
                 syslog_err("[facilities] encoding BTPDataRequest for cam failed");
                 continue;

src/cam.h

@@ -24,6 +24,9 @@ typedef struct lightship {
     uint32_t vehicle_gen_max;
     uint32_t rsu_gen_min;
     uint32_t rsu_vehicle_permanence;
+
+    ProtectedCommunicationZone_t ** pz;
+    uint16_t pz_len;
 } lightship_t;
 
 lightship_t* lightship_init();

src/config.c

@@ -6,6 +6,8 @@
 #include <stdlib.h>
 #include <it2s-gps.h>
 #include <it2s-tiles.h>
+#include <dirent.h>
+#include <camv2/ProtectedCommunicationZone.h>
 
 #define syslog_emerg(msg, ...) syslog(LOG_EMERG, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__)
 #define syslog_err(msg, ...) syslog(LOG_ERR, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__)
@@ -86,7 +88,7 @@ int itss_config(void* facilities_s, char* config_file) {
     fclose(fp);
 
     // Tables
-    toml_table_t *general, *security, *denm, *cam, *beacon, *replay, *ivim;
+    toml_table_t *general, *security, *denm, *cam, *beacon, *replay, *ivim, *protected_zones;
 
     if (0 == (general = toml_table_in(conf, "general"))) {syslog_err("[facilities] [config] failed locating [general] table"); return 1;}
     if (0 == (security = toml_table_in(conf, "security"))) {syslog_err("[facilities] [config] failed locating [security] table"); return 1;}
@@ -94,6 +96,7 @@ int itss_config(void* facilities_s, char* config_file) {
    if (0 == (cam = toml_table_in(conf, "cam"))) {syslog_err("[facilities] [config] failed locating [cam] table"); return 1;}
    if (0 == (replay = toml_table_in(conf, "replay"))) {syslog_err("[facilities] [config] failed locating [replay] table"); return 1;}
    if (0 == (ivim = toml_table_in(conf, "ivim"))) {syslog_err("[facilities] [config] failed locating [ivim] table"); return 1;}
+   if (0 == (protected_zones = toml_table_in(conf, "protected-zones"))) {syslog_err("[facilities] [config] failed locating [protected-zones] table"); return 1;}
 
 
 
@@ -170,6 +173,46 @@ int itss_config(void* facilities_s, char* config_file) {
     int replay_active = 1;
     rv += extract_val_bool(&replay_active, replay, "activate");
     facilities->replay = replay_active;
+
+    // PZ
+    if (facilities->station_type == 15) {
+        char *pz_path;
+        rv += extract_val_string(&pz_path, protected_zones, "path");
+
+        int i = 0;
+
+        DIR *d = opendir(pz_path);
+        struct dirent *dir;
+        char file[256];
+        char pz_xml[2048];
+        if (d) {
+            while ((dir = readdir(d)) != NULL && i < 16) {
+                sprintf(file, "%s/%s", pz_path, dir->d_name);
+                FILE *fp = fopen(file, "r");
+                if (!fp) continue;
+                fseek(fp, 0, SEEK_END);
+                uint16_t size = ftell(fp);
+                fseek(fp, 0, SEEK_SET);
+                if (!fread(pz_xml, 1, size, fp)) continue;
+                fclose(fp);
+               
+                ProtectedCommunicationZone_t *zone = calloc(1, sizeof(ProtectedCommunicationZone_t));
+
+                asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void**) &zone, pz_xml, size);
+                if (!dec.code) {
+                    facilities->lightship->pz[i] = zone;
+                    ++facilities->lightship->pz_len;
+                    ++i;
+                } else {
+                    ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, zone);
+                }
+
+            }
+            closedir(d);
+        }
+        free(pz_path);
+    }
+