TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Protected zones

This commit is contained in:
emanuel 2020-11-30 19:35:20 +00:00
parent 8a9aed7fcd
commit 18ae6f1182
4 changed files with 146 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/CMakeLists.txt
View File

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

107
src/cam.c
View File

@ -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;

3
src/cam.h
View File

@ -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();

45
src/config.c
View File

@ -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;}
@ -171,6 +174,46 @@ int itss_config(void* facilities_s, char* config_file) {
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);
}