diff --git a/src/cam.c b/src/cam.c index b6f6b7e..8174413 100644 --- a/src/cam.c +++ b/src/cam.c @@ -1,6 +1,7 @@ #include "cam.h" #include "facilities.h" +#include #include #include #include @@ -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) {