From 18ae6f1182445a93682a152cb0dafd87b081b529 Mon Sep 17 00:00:00 2001 From: emanuel Date: Mon, 30 Nov 2020 19:35:20 +0000 Subject: [PATCH] Protected zones --- src/CMakeLists.txt | 1 + src/cam.c | 107 +++++++++++++++++++++++++++++++++++++++++---- src/cam.h | 3 ++ src/config.c | 45 ++++++++++++++++++- 4 files changed, 146 insertions(+), 10 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 59c661c..0e02a90 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -18,6 +18,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities -lit2s-asn-camv2 -lit2s-asn-ivim -lit2s-asn-denmv2 + -lm ) INSTALL( diff --git a/src/cam.c b/src/cam.c index df5f910..97558fa 100644 --- a/src/cam.c +++ b/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); - if (facilities->lightship->type == StationType_roadSideUnit) { + pthread_mutex_lock(&lightship->lock); + if (lightship->type == StationType_roadSideUnit) { + // Send CAMs if vehicles nearby if (bdi->stationType != StationType_roadSideUnit && bdi->isNeighbour) { - facilities->lightship->last_vehicle = now; - facilities->lightship->is_vehicle_near = true; + lightship->last_vehicle = now; + 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; diff --git a/src/cam.h b/src/cam.h index 20ed08a..40c663b 100644 --- a/src/cam.h +++ b/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(); diff --git a/src/config.c b/src/config.c index c5fd6b6..71f8c19 100644 --- a/src/config.c +++ b/src/config.c @@ -6,6 +6,8 @@ #include #include #include +#include +#include #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); + } +