535 lines
26 KiB
C
535 lines
26 KiB
C
#include "cam.h"
|
|
#include "facilities.h"
|
|
|
|
#include <camv2/INTEGER.h>
|
|
#include <itss-transport/BTPDataRequest.h>
|
|
#include <itss-transport/BTPDataIndication.h>
|
|
#include <camv2/CAM.h>
|
|
#include <it2s-gps.h>
|
|
|
|
#include <stdint.h>
|
|
#include <time.h>
|
|
#include <zmq.h>
|
|
#include <unistd.h>
|
|
#include <syslog.h>
|
|
#include <math.h>
|
|
|
|
#define syslog_info(msg, ...) syslog(LOG_INFO, msg, ##__VA_ARGS__)
|
|
#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__)
|
|
|
|
#ifndef NDEBUG
|
|
#define syslog_debug(msg, ...) syslog(LOG_DEBUG, "%s:%d " msg "", __func__, __LINE__, ##__VA_ARGS__)
|
|
#else
|
|
#define syslog_debug(msg, ...)
|
|
#endif
|
|
|
|
#define LEAP_SECONDS 5
|
|
|
|
#define EARTH_RADIUS 6369000
|
|
#define RAD_PER_DEG M_PI_2/180.0
|
|
|
|
const cid_ssp_bm_t CID_SSP_BM_MAP[] = {
|
|
{"CenDsrcTollingZone/ProtectedCommunicationZonesRSU", CID_PROTECTED_ZONES, 0x8000},
|
|
{"publicTransport/publicTransportContainer", CID_PUBLIC_TRANSPORT, 0x4000},
|
|
{"specialTransport/specialTransportContainer", CID_SPECIAL_TRANSPORT, 0x2000},
|
|
{"dangerousGoods/dangerousGoodsContainer", CID_DANGEROUS_GOODS, 0x1000},
|
|
{"roadwork/roadWorksContainerBasic", CID_ROADWORK, 0x0800},
|
|
{"rescue/rescueContainer", CID_RESCUE, 0x0400},
|
|
{"emergency/emergencyContaine", CID_EMERGENCY, 0x0200},
|
|
{"safetyCar/safetyCarContainer", CID_SAFETY_CAR, 0x0100},
|
|
{"closedLanes/RoadworksContainerBasic", CID_CLOSED_LANES, 0x0080},
|
|
{"requestForRightOfWay/EmergencyContainer: EmergencyPriority", CID_REQUEST_FOR_RIGHT_OF_WAY, 0x0040},
|
|
{"requestForFreeCrossingAtATrafficLight/EmergencyContainer: EmergencyPriority", CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, 0x0020},
|
|
{"noPassing/SafetyCarContainer: TrafficRule", CID_NO_PASSING, 0x0010},
|
|
{"noPassingForTrucks/SafetyCarContainer: TrafficRule", CID_NO_PASSING_FOR_TRUCKS, 0x0008},
|
|
{"speedLimit/SafetyCarContainer", CID_SPEED_LIMIT, 0x0004},
|
|
{"reserved0", CID_RESERVED, 0x0002},
|
|
{"reserved1", CID_RESERVED, 0x0001},
|
|
};
|
|
|
|
static int permissions_check(int cid, uint8_t* permissions, uint8_t permissions_len) {
|
|
/* CAM SSP scheme
|
|
*
|
|
* byte | description
|
|
* ---------------------------------
|
|
* 0 | SSP version control
|
|
* 1-2 | Service-specific parameter
|
|
* 3-30 | Reserved
|
|
*/
|
|
|
|
if (permissions_len < 3) {
|
|
syslog_debug("[facilities] [ca] permissions length too small");
|
|
return 0;
|
|
}
|
|
|
|
uint32_t perms_int = (*((uint32_t*) permissions)) >> 16;
|
|
|
|
uint32_t perm_val;
|
|
for (int i = 0; i < 16; ++i) {
|
|
if (cid == CID_SSP_BM_MAP[i].cid) {
|
|
perm_val = CID_SSP_BM_MAP[i].bitmap_val;
|
|
}
|
|
}
|
|
|
|
if ((perm_val & perms_int) == perm_val) return 1;
|
|
else return 0;
|
|
|
|
}
|
|
|
|
static AltitudeConfidence_t getAltitudeConfidence(double conf) {
|
|
if (conf >= 200) return AltitudeConfidence_outOfRange;
|
|
if (conf >= 100) return AltitudeConfidence_alt_200_00;
|
|
if (conf >= 50) return AltitudeConfidence_alt_100_00;
|
|
if (conf >= 20) return AltitudeConfidence_alt_050_00;
|
|
if (conf >= 10) return AltitudeConfidence_alt_020_00;
|
|
if (conf >= 5) return AltitudeConfidence_alt_010_00;
|
|
if (conf >= 1) return AltitudeConfidence_alt_005_00;
|
|
|
|
return AltitudeConfidence_alt_001_00;
|
|
}
|
|
|
|
static HeadingConfidence_t getHeadingConfidence(uint32_t conf) {
|
|
if (conf > 125) return 126;
|
|
if (conf == 125) return 125;
|
|
if (conf == 0) return 127;
|
|
if (conf < 1) return 1;
|
|
|
|
return conf;
|
|
}
|
|
|
|
static SpeedConfidence_t getSpeedConfidence(uint32_t conf) {
|
|
if (conf > 125) return 126;
|
|
if (conf == 125) return 125;
|
|
if (conf == 0) return 127;
|
|
if (conf < 1) return 1;
|
|
|
|
return conf;
|
|
}
|
|
|
|
|
|
static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
|
|
int rv = 0;
|
|
|
|
struct timespec systemtime;
|
|
if (clock_gettime(CLOCK_REALTIME, &systemtime)) {
|
|
syslog_emerg("clock_gettime() failed");
|
|
}
|
|
|
|
struct it2s_gps_data gps_data;
|
|
it2s_gps_read(&gps_data);
|
|
|
|
CAM_t *cam_tx = calloc(1, sizeof(CAM_t));
|
|
|
|
cam_tx->header.protocolVersion = 2;
|
|
cam_tx->header.messageID = ItsPduHeader__messageID_cam;
|
|
pthread_mutex_lock(&facilities->lock);
|
|
cam_tx->header.stationID = facilities->station_id;
|
|
pthread_mutex_unlock(&facilities->lock);
|
|
cam_tx->cam.camParameters.basicContainer.stationType = facilities->station_type;
|
|
|
|
if (facilities->station_type != StationType_roadSideUnit) {
|
|
cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleWidth = 20;
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthValue = 46;
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable;
|
|
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.driveDirection = DriveDirection_unavailable;
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureValue = CurvatureValue_unavailable;
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureConfidence = CurvatureConfidence_unavailable;
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateValue = YawRateValue_unavailable;
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateConfidence = YawRateConfidence_unavailable;
|
|
|
|
if (!isnan(gps_data.gps_track)) {
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = ((uint32_t)(gps_data.gps_track * 10));
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = getHeadingConfidence((uint32_t)(gps_data.gps_epd * 10));
|
|
} else {
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = HeadingValue_unavailable;
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = HeadingConfidence_unavailable;
|
|
|
|
}
|
|
|
|
if (!isnan(gps_data.gps_speed)) {
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = ((uint32_t)(gps_data.gps_speed * 100)); // cm/s
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = getSpeedConfidence((uint32_t)(gps_data.gps_eps * 100));
|
|
} else {
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = SpeedValue_unavailable;
|
|
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = SpeedConfidence_unavailable;
|
|
}
|
|
} else {
|
|
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));
|
|
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) {
|
|
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) {
|
|
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) {
|
|
pzs->list.array[i]->protectedZoneID = malloc(8);
|
|
*pzs->list.array[i]->protectedZoneID = *facilities->lightship->pz[i]->protectedZoneID;
|
|
}
|
|
if (facilities->lightship->pz[i]->protectedZoneRadius) {
|
|
pzs->list.array[i]->protectedZoneRadius = malloc(8);
|
|
*pzs->list.array[i]->protectedZoneRadius = *facilities->lightship->pz[i]->protectedZoneRadius;
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
long generationdeltatime = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
|
|
generationdeltatime = generationdeltatime - 1072915200000; // EPOCH -> 2004/01/01 00:00:000
|
|
generationdeltatime = generationdeltatime % 64536;
|
|
cam_tx->cam.generationDeltaTime = generationdeltatime;
|
|
|
|
if (!isnan(gps_data.gps_altitude)) {
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = (int32_t)((gps_data.gps_altitude) * 10);
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = getAltitudeConfidence(gps_data.gps_epv);
|
|
} else {
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = AltitudeValue_unavailable;
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable;
|
|
}
|
|
|
|
if (!facilities->gps_fixed) {
|
|
if (!isnan(gps_data.gps_latitude)) {
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = (int32_t)((gps_data.gps_latitude) * 10000000);
|
|
} else {
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = DeltaLatitude_unavailable;
|
|
}
|
|
|
|
if (!isnan(gps_data.gps_longitude)) {
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = (int32_t)((gps_data.gps_longitude) * 10000000);
|
|
} else {
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = Longitude_unavailable;
|
|
}
|
|
} else {
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = (int32_t)((facilities->latitude) * 10000000);
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = (int32_t)((facilities->longitude) * 10000000);
|
|
|
|
}
|
|
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
|
|
cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
|
|
|
|
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_CAM, NULL, cam_tx, cam, 256);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] [ca] failed encoding CAM (%s)", enc.failed_type->name);
|
|
rv = 1;
|
|
goto cleanup;
|
|
}
|
|
*cam_len = (enc.encoded + 7) / 8;
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_CAM, cam_tx);
|
|
|
|
return rv;
|
|
}
|
|
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
int lightship_check(lightship_t* lightship) {
|
|
int rv = 0;
|
|
|
|
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(&lightship->lock);
|
|
|
|
if (lightship->type == StationType_roadSideUnit) { // RSU
|
|
if (lightship->is_vehicle_near && now > lightship->next_cam) {
|
|
rv = 1;
|
|
}
|
|
} else { // Vehicle
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
pthread_mutex_unlock(&lightship->lock);
|
|
|
|
return rv;
|
|
}
|
|
|
|
void lightship_reset_timer(lightship_t* 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(&lightship->lock);
|
|
|
|
if (lightship->type != StationType_roadSideUnit) { // Vehicle
|
|
lightship->next_cam = now + lightship->vehicle_gen_max;
|
|
} else { // RSU
|
|
if (now > lightship->last_vehicle + lightship->rsu_vehicle_permanence) {
|
|
lightship->is_vehicle_near = false;
|
|
}
|
|
lightship->next_cam = now + lightship->rsu_gen_min;
|
|
}
|
|
|
|
pthread_mutex_unlock(&lightship->lock);
|
|
}
|
|
|
|
int check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam, ServiceSpecificPermissions_t* ssp) {
|
|
int rv = 0;
|
|
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;
|
|
|
|
// Check permissions
|
|
if (ssp) {
|
|
if (cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
|
|
if (!permissions_check(CID_PROTECTED_ZONES, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
}
|
|
if (cam->cam.camParameters.specialVehicleContainer) {
|
|
switch (cam->cam.camParameters.specialVehicleContainer->present) {
|
|
case SpecialVehicleContainer_PR_NOTHING:
|
|
break;
|
|
case SpecialVehicleContainer_PR_publicTransportContainer:
|
|
if (!permissions_check(CID_PUBLIC_TRANSPORT, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
break;
|
|
case SpecialVehicleContainer_PR_specialTransportContainer:
|
|
if (!permissions_check(CID_SPECIAL_TRANSPORT, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
break;
|
|
case SpecialVehicleContainer_PR_dangerousGoodsContainer:
|
|
if (!permissions_check(CID_DANGEROUS_GOODS, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
break;
|
|
case SpecialVehicleContainer_PR_roadWorksContainerBasic:
|
|
if (!permissions_check(CID_ROADWORK, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
if (cam->cam.camParameters.specialVehicleContainer->choice.roadWorksContainerBasic.closedLanes) {
|
|
if (!permissions_check(CID_CLOSED_LANES, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
}
|
|
break;
|
|
case SpecialVehicleContainer_PR_rescueContainer:
|
|
if (!permissions_check(CID_RESCUE, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
break;
|
|
case SpecialVehicleContainer_PR_emergencyContainer:
|
|
if (!permissions_check(CID_EMERGENCY, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
if (cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority &&
|
|
cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf) {
|
|
// TODO verify bitmap
|
|
uint8_t bm = *cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf;
|
|
if (bm & 0x02) {
|
|
if (!permissions_check(CID_REQUEST_FOR_RIGHT_OF_WAY, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
}
|
|
if (bm & 0x01) {
|
|
if (!permissions_check(CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
}
|
|
}
|
|
break;
|
|
case SpecialVehicleContainer_PR_safetyCarContainer:
|
|
if (!permissions_check(CID_SAFETY_CAR, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) {
|
|
switch (*cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) {
|
|
case TrafficRule_noPassing:
|
|
if (!permissions_check(CID_NO_PASSING, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
break;
|
|
case TrafficRule_noPassingForTrucks:
|
|
if (!permissions_check(CID_NO_PASSING_FOR_TRUCKS, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.speedLimit) {
|
|
if (!permissions_check(CID_SPEED_LIMIT, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
pthread_mutex_lock(&lightship->lock);
|
|
if (lightship->type == StationType_roadSideUnit) {
|
|
// Send CAMs if vehicles nearby
|
|
if (bdi->stationType != StationType_roadSideUnit && bdi->isNeighbour) {
|
|
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 k = 0; k < pzs->list.count; ++k) {
|
|
|
|
bool found = false;
|
|
for (int j = 0; j < lightship->pz_len; ++j) {
|
|
if (lightship->pz[j]->protectedZoneLatitude == pzs->list.array[k]->protectedZoneLatitude &&
|
|
lightship->pz[j]->protectedZoneLongitude == pzs->list.array[k]->protectedZoneLongitude) {
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
if (found) continue;
|
|
|
|
lightship->pz[k + lightship->pz_len] = calloc(1, sizeof(ProtectedCommunicationZone_t));
|
|
lightship->pz[k + lightship->pz_len]->protectedZoneLatitude = pzs->list.array[k]->protectedZoneLatitude;
|
|
lightship->pz[k + lightship->pz_len]->protectedZoneLongitude = pzs->list.array[k]->protectedZoneLongitude;
|
|
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->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);
|
|
*lightship->pz[k + lightship->pz_len]->protectedZoneID = *pzs->list.array[k]->protectedZoneID;
|
|
}
|
|
if (pzs->list.array[k]->protectedZoneRadius) {
|
|
lightship->pz[k + lightship->pz_len]->protectedZoneRadius = malloc(8);
|
|
*lightship->pz[k + lightship->pz_len]->protectedZoneRadius = *pzs->list.array[k]->protectedZoneRadius;
|
|
}
|
|
++lightship->pz_len;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
pthread_mutex_unlock(&lightship->lock);
|
|
|
|
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;
|
|
uint8_t code = 0;
|
|
facilities_t *facilities = (facilities_t*) fc;
|
|
|
|
void* h = zmq_socket(facilities->ctx, ZMQ_REQ);
|
|
zmq_connect(h, TRANSPORT_ADDRESS);
|
|
|
|
BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t));
|
|
|
|
bdr->btpType = BTPType_btpB;
|
|
|
|
bdr->gnDestinationAddress.buf = malloc(6);
|
|
for (int i = 0; i < 6; ++i) {bdr->gnDestinationAddress.buf[i] = 0xff;}
|
|
bdr->gnDestinationAddress.size = 6;
|
|
|
|
bdr->gnPacketTransportType = PacketTransportType_shb;
|
|
|
|
bdr->destinationPort = Port_cam;
|
|
|
|
bdr->gnTrafficClass = 2;
|
|
|
|
bdr->data.buf = malloc(384);
|
|
|
|
if (facilities->use_security) {
|
|
bdr->gnSecurityProfile = malloc(sizeof(long));
|
|
*bdr->gnSecurityProfile = 1;
|
|
}
|
|
|
|
uint8_t bdr_oer[512];
|
|
bdr_oer[0] = 4; // Facilities
|
|
while (!facilities->exit) {
|
|
usleep(1000*50);
|
|
|
|
if (lightship_check(facilities->lightship)) {
|
|
rv = mk_cam(facilities, bdr->data.buf, (uint32_t *) &bdr->data.size);
|
|
if (rv) {
|
|
continue;
|
|
}
|
|
|
|
// Check if inside PZ
|
|
bdr->gnCommunicationProfile = 0;
|
|
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) {
|
|
syslog_err("[facilities] encoding BTPDataRequest for cam failed");
|
|
continue;
|
|
}
|
|
|
|
queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3);
|
|
pthread_cond_signal(&facilities->tx_queue->trigger);
|
|
|
|
lightship_reset_timer(facilities->lightship);
|
|
}
|
|
|
|
}
|
|
|
|
ASN_STRUCT_FREE(asn_DEF_BTPDataRequest, bdr);
|
|
|
|
return NULL;
|
|
}
|