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

Add PZs expiry time logic

This commit is contained in:
emanuel 2020-12-01 13:27:20 +00:00
parent 50aeeea0d0
commit 943a976cae
1 changed files with 67 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
src/cam.c
View File

@ -1,6 +1,7 @@
#include "cam.h"
#include "facilities.h"
#include <camv2/INTEGER.h>
#include <itss-transport/BTPDataRequest.h>
#include <itss-transport/BTPDataIndication.h>
#include <camv2/CAM.h>
@ -113,27 +114,28 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency;
if (facilities->lightship->pz_len > 0) {
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*));
ProtectedCommunicationZonesRSU_t *pzs = cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
pzs->list.count = facilities->lightship->pz_len;
pzs->list.size = facilities->lightship->pz_len * sizeof(void*);
pzs->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;
pzs->list.array[i] = calloc(1, sizeof(ProtectedCommunicationZone_t));
pzs->list.array[i]->protectedZoneLatitude = facilities->lightship->pz[i]->protectedZoneLatitude;
pzs->list.array[i]->protectedZoneLongitude = facilities->lightship->pz[i]->protectedZoneLongitude;
pzs->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;
pzs->list.array[i]->expiryTime->size = facilities->lightship->pz[i]->expiryTime->size;
pzs->list.array[i]->expiryTime->buf = malloc(facilities->lightship->pz[i]->expiryTime->size);
memcpy(pzs->list.array[i]->expiryTime->buf, facilities->lightship->pz[i]->expiryTime->buf, facilities->lightship->pz[i]->expiryTime->size);
}
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;
pzs->list.array[i]->protectedZoneID = malloc(8);
*pzs->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;
pzs->list.array[i]->protectedZoneRadius = malloc(8);
*pzs->list.array[i]->protectedZoneRadius = *facilities->lightship->pz[i]->protectedZoneRadius;
}
}
}
@ -212,6 +214,20 @@ int lightship_check(lightship_t* lightship) {
if (now > lightship->next_cam) {
rv = 1;
}
// Remove expired PZs
for (int i = 0; i < lightship->pz_len; ++i) {
uint64_t expiry;
if (lightship->pz[i]->expiryTime) {
asn_INTEGER2ulong(lightship->pz[i]->expiryTime, &expiry);
if (now >= expiry) {
ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, lightship->pz[i]);
for (int j = i; j < lightship->pz_len - 1; ++j) {
lightship->pz[j] = lightship->pz[j+1];
}
--lightship->pz_len;
}
}
}
}
@ -280,8 +296,9 @@ int check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam) {
lightship->pz[k + lightship->pz_len]->protectedZoneType = pzs->list.array[k]->protectedZoneType;
if (pzs->list.array[k]->expiryTime) {
lightship->pz[k + lightship->pz_len]->expiryTime = malloc(8);
*lightship->pz[k + lightship->pz_len]->expiryTime = *pzs->list.array[k]->expiryTime;
lightship->pz[k + lightship->pz_len]->expiryTime->size = pzs->list.array[k]->expiryTime->size;
lightship->pz[k + lightship->pz_len]->expiryTime->buf = malloc(pzs->list.array[k]->expiryTime->size);
memcpy(lightship->pz[k + lightship->pz_len]->expiryTime->buf, pzs->list.array[k]->expiryTime->buf, pzs->list.array[k]->expiryTime->size);
}
if (pzs->list.array[k]->protectedZoneID) {
lightship->pz[k + lightship->pz_len]->protectedZoneID = malloc(8);
@ -301,6 +318,39 @@ int check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam) {
return rv;
}
static int check_pz(lightship_t *lightship) {
bool is_inside = false;
struct it2s_gps_data gps_data;
it2s_gps_read(&gps_data);
pthread_mutex_lock(&lightship->lock);
for (int i = 0; i < lightship->pz_len; ++i) {
double d_lat = (gps_data.gps_latitude - (double) lightship->pz[i]->protectedZoneLatitude/10000000) * RAD_PER_DEG;
double d_lon = (gps_data.gps_longitude - (double) 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) 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 (lightship->pz[i]->protectedZoneRadius) {
pz_radius = *lightship->pz[i]->protectedZoneRadius;
}
if (d < pz_radius) {
is_inside = true;
}
}
pthread_mutex_unlock(&lightship->lock);
return is_inside;
}
void *ca_service(void *fc) {
int rv = 0;
@ -345,32 +395,7 @@ void *ca_service(void *fc) {
// 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;
}
}
}
if (facilities->station_type != 15 && check_pz(facilities->lightship)) bdr->gnCommunicationProfile = 1;
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 511);
if (enc.encoded == -1) {