794 lines
38 KiB
C
794 lines
38 KiB
C
#include "cam.h"
|
|
#include "facilities.h"
|
|
|
|
#include <camv2/INTEGER.h>
|
|
#include <itss-transport/TransportRequest.h>
|
|
#include <itss-facilities/FacilitiesIndication.h>
|
|
#include <itss-management/ManagementRequest.h>
|
|
#include <camv2/CAM.h>
|
|
|
|
#include <sys/mman.h>
|
|
#include <stdint.h>
|
|
#include <time.h>
|
|
#include <zmq.h>
|
|
#include <unistd.h>
|
|
#include <syslog.h>
|
|
#include <math.h>
|
|
#include <fcntl.h>
|
|
|
|
#include <it2s-tender/space.h>
|
|
#include <it2s-tender/time.h>
|
|
|
|
#include <it2s-obd/shmem.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/180.0
|
|
|
|
double haversine(double lat1, double lon1, double lat2, double lon2) {
|
|
double d_lat = (lat1-lat2) * RAD_PER_DEG;
|
|
double d_lon = (lon1-lon2) * RAD_PER_DEG;
|
|
|
|
double a = pow(sin(d_lat/2.0), 2) + cos(lat1 * RAD_PER_DEG) * cos(lat2 * RAD_PER_DEG) * pow(sin(d_lon/2.0), 2);
|
|
double c = 2 * atan2(sqrt(a), sqrt(1-a));
|
|
return EARTH_RADIUS * c;
|
|
}
|
|
|
|
|
|
const cid_ssp_bm_t CID_SSP_BM_MAP[] = {
|
|
{"CenDsrcTollingZone/ProtectedCommunicationZonesRSU", 0x8000},
|
|
{"publicTransport/publicTransportContainer", 0x4000},
|
|
{"specialTransport/specialTransportContainer", 0x2000},
|
|
{"dangerousGoods/dangerousGoodsContainer", 0x1000},
|
|
{"roadwork/roadWorksContainerBasic", 0x0800},
|
|
{"rescue/rescueContainer", 0x0400},
|
|
{"emergency/emergencyContainer", 0x0200},
|
|
{"safetyCar/safetyCarContainer", 0x0100},
|
|
{"closedLanes/RoadworksContainerBasic", 0x0080},
|
|
{"requestForRightOfWay/EmergencyContainer: EmergencyPriority", 0x0040},
|
|
{"requestForFreeCrossingAtATrafficLight/EmergencyContainer: EmergencyPriority", 0x0020},
|
|
{"noPassing/SafetyCarContainer: TrafficRule", 0x0010},
|
|
{"noPassingForTrucks/SafetyCarContainer: TrafficRule", 0x0008},
|
|
{"speedLimit/SafetyCarContainer", 0x0004},
|
|
{"reserved0", 0x0002},
|
|
{"reserved1", 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;
|
|
}
|
|
|
|
uint16_t perms_int = *(uint16_t*)(permissions+1);
|
|
|
|
uint16_t perm_val = CID_SSP_BM_MAP[cid].bitmap_val;
|
|
perm_val = (perm_val>>8) | (perm_val<<8);
|
|
|
|
if ((perm_val & perms_int) == perm_val) return 1;
|
|
else return 0;
|
|
|
|
}
|
|
|
|
static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) {
|
|
int rv = 0;
|
|
int shm_fd, shm_valid = 0;
|
|
decoded_can_values_shm* shared_message;
|
|
|
|
if(facilities->lightship->use_obd){
|
|
shm_fd = shm_open("it2s-bluetooth-decoded", O_RDONLY, 0666);
|
|
if(shm_fd == -1)
|
|
syslog_err("[facilities] failed to open CAN shared memory\n");
|
|
shared_message = (decoded_can_values_shm *)mmap(0, sizeof(decoded_can_values_shm), PROT_READ, MAP_SHARED, shm_fd, 0);
|
|
if(shared_message == MAP_FAILED)
|
|
syslog_err("[facilities] failed to map CAN shared memory\n");
|
|
}
|
|
|
|
CAM_t *cam = calloc(1, sizeof(CAM_t));
|
|
|
|
cam->header.protocolVersion = 2;
|
|
cam->header.messageID = ItsPduHeader__messageID_cam;
|
|
pthread_mutex_lock(&facilities->id.lock);
|
|
cam->header.stationID = facilities->id.station_id;
|
|
pthread_mutex_unlock(&facilities->id.lock);
|
|
cam->cam.camParameters.basicContainer.stationType = facilities->station_type;
|
|
|
|
BasicContainer_t* bc = &cam->cam.camParameters.basicContainer;
|
|
|
|
uint64_t now = it2s_tender_get_clock(&facilities->epv);
|
|
|
|
lightship_t* lightship = facilities->lightship;
|
|
pthread_mutex_lock(&lightship->lock);
|
|
|
|
if (facilities->station_type != StationType_roadSideUnit) {
|
|
cam->cam.generationDeltaTime = now % 65536;
|
|
|
|
it2s_tender_lock_space(&facilities->epv);
|
|
it2s_tender_get_space(&facilities->epv);
|
|
bc->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude;
|
|
bc->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf;
|
|
|
|
// Set GPS coordinates
|
|
bc->referencePosition.latitude = facilities->epv.space.latitude;
|
|
bc->referencePosition.longitude = facilities->epv.space.longitude;
|
|
uint16_t lat_conf = facilities->epv.space.latitude_conf;
|
|
uint16_t lon_conf = facilities->epv.space.longitude_conf;
|
|
|
|
cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
|
|
BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
|
|
|
|
if(facilities->lightship->use_obd){
|
|
uint8_t ac = 0x00;
|
|
|
|
// Speed (already getting set bellow)
|
|
// bvc_hf.speed.speedValue = ceil(shared_message->speed_value ÷ 0.036);
|
|
// bvc_hf_speed.speedConfidence = 1;
|
|
|
|
// Steering Wheel Angle
|
|
if(shared_message->w_angle_value != 1683){
|
|
bvc_hf->steeringWheelAngle = calloc(1, sizeof(SteeringWheelAngle_t));
|
|
bvc_hf->steeringWheelAngle->steeringWheelAngleValue = shared_message->w_angle_value;
|
|
bvc_hf->steeringWheelAngle->steeringWheelAngleConfidence = 1;
|
|
}
|
|
// still missing temperature; where is de_temperature ?
|
|
|
|
// Acceleration Control (encondig failure)
|
|
if(shared_message->b_pedal_value != 1683 && shared_message->s_pedal_value != 1683){
|
|
if(shared_message->b_pedal_value)
|
|
ac = ac | 0x80;
|
|
if(shared_message->s_pedal_value)
|
|
ac = ac | 0x40;
|
|
bvc_hf->accelerationControl = calloc(1, sizeof(AccelerationControl_t));
|
|
bvc_hf->accelerationControl->buf = malloc(sizeof(uint8_t));
|
|
memcpy(bvc_hf->accelerationControl->buf, &ac, 1);
|
|
bvc_hf->accelerationControl->size = 1;
|
|
bvc_hf->accelerationControl->bits_unused = 1;
|
|
}
|
|
}
|
|
|
|
// Set speed
|
|
bvc_hf->speed.speedValue = facilities->epv.space.speed;
|
|
bvc_hf->speed.speedConfidence = facilities->epv.space.speed_conf;
|
|
|
|
// Set heading
|
|
bvc_hf->heading.headingValue = facilities->epv.space.heading;
|
|
bvc_hf->heading.headingConfidence = facilities->epv.space.heading_conf;
|
|
|
|
it2s_tender_unlock_space(&facilities->epv);
|
|
|
|
if (lat_conf > lon_conf) {
|
|
bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
|
|
bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf;
|
|
bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 0;
|
|
} else {
|
|
bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
|
|
bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf;
|
|
bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 900;
|
|
}
|
|
|
|
bvc_hf->vehicleWidth = facilities->vehicle.width;
|
|
bvc_hf->vehicleLength.vehicleLengthValue = facilities->vehicle.length;
|
|
bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;
|
|
bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
|
|
bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable;
|
|
|
|
bvc_hf->driveDirection = DriveDirection_unavailable;
|
|
bvc_hf->curvature.curvatureValue = CurvatureValue_unavailable;
|
|
bvc_hf->curvature.curvatureConfidence = CurvatureConfidence_unavailable;
|
|
bvc_hf->yawRate.yawRateValue = YawRateValue_unavailable;
|
|
bvc_hf->yawRate.yawRateConfidence = YawRateConfidence_unavailable;
|
|
|
|
// Save current values
|
|
if (lightship->pos_history_len == POS_HISTORY_MAX_LEN) {
|
|
free(lightship->pos_history[POS_HISTORY_MAX_LEN-1]);
|
|
--lightship->pos_history_len;
|
|
}
|
|
|
|
for (int i = lightship->pos_history_len-1; i >= 0; --i) {
|
|
lightship->pos_history[i+1] = lightship->pos_history[i];
|
|
}
|
|
|
|
lightship->pos_history[0] = malloc(sizeof(pos_vector_t));
|
|
lightship->pos_history[0]->heading = bvc_hf->heading.headingValue;
|
|
lightship->pos_history[0]->lat = bc->referencePosition.latitude;
|
|
lightship->pos_history[0]->lon = bc->referencePosition.longitude;
|
|
lightship->pos_history[0]->alt = bc->referencePosition.altitude.altitudeValue;
|
|
lightship->pos_history[0]->speed = bvc_hf->speed.speedValue;
|
|
|
|
lightship->last_cam = now;
|
|
lightship->pos_history[0]->ts = now;
|
|
|
|
++lightship->pos_history_len;
|
|
|
|
// Acceleration
|
|
if (lightship->pos_history_len > 1) {
|
|
double delta_angle = (lightship->pos_history[0]->heading - lightship->pos_history[1]->heading) / 10.0; /* 1º */
|
|
double delta_speed = (lightship->pos_history[0]->speed - (lightship->pos_history[1]->speed * cos(delta_angle * M_PI/180))) / 10.0; /* 0.1 m/s */
|
|
int16_t long_a = (int16_t) (delta_speed / (double) ((lightship->pos_history[0]->ts - lightship->pos_history[1]->ts) / 1000)); /* 0.1 m/s^2 */
|
|
|
|
if (long_a > 160) long_a = 160;
|
|
else if (long_a < -160) long_a = -160;
|
|
|
|
bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = long_a;
|
|
bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable;
|
|
} else {
|
|
bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
|
|
bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable;
|
|
}
|
|
|
|
// Low frequency container
|
|
if (now > lightship->last_cam_lfc + 500) {
|
|
cam->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(LowFrequencyContainer_t));
|
|
|
|
cam->cam.camParameters.lowFrequencyContainer->present = LowFrequencyContainer_PR_basicVehicleContainerLowFrequency;
|
|
|
|
BasicVehicleContainerLowFrequency_t* bvc_lf = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency;
|
|
|
|
if(facilities->lightship->use_obd){
|
|
uint8_t el = 0x00;
|
|
|
|
// Exterior Lights
|
|
if(shared_message->lights_value != 1683){
|
|
if(shared_message->lights_value)
|
|
el = el | 0x80;
|
|
bvc_lf->exteriorLights.buf = malloc(sizeof(uint8_t));
|
|
memcpy(bvc_lf->exteriorLights.buf, &el, 1);
|
|
bvc_lf->exteriorLights.size = 1;
|
|
bvc_lf->exteriorLights.bits_unused = 0;
|
|
}
|
|
}
|
|
|
|
PathHistory_t* ph = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory;
|
|
|
|
ph->list.array = malloc((lightship->pos_history_len-1) * sizeof(void*));
|
|
ph->list.count = lightship->pos_history_len-1;
|
|
ph->list.size = (lightship->pos_history_len-1) * sizeof(void*);
|
|
|
|
for (int i = 0; i < lightship->pos_history_len-1; ++i) {
|
|
ph->list.array[i] = calloc(1,sizeof(PathPoint_t));
|
|
|
|
if (lightship->pos_history[i+1]->alt != AltitudeValue_unavailable && lightship->pos_history[i]->alt != AltitudeValue_unavailable) {
|
|
ph->list.array[i]->pathPosition.deltaAltitude = lightship->pos_history[i+1]->alt - lightship->pos_history[i]->alt;
|
|
} else {
|
|
ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
|
|
}
|
|
|
|
if (lightship->pos_history[i+1]->lat != Latitude_unavailable && lightship->pos_history[i]->lat != Latitude_unavailable) {
|
|
ph->list.array[i]->pathPosition.deltaLatitude = lightship->pos_history[i+1]->lat - lightship->pos_history[i]->lat;
|
|
} else {
|
|
ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
|
|
}
|
|
|
|
if (lightship->pos_history[i+1]->lon != Longitude_unavailable && lightship->pos_history[i]->lon != Longitude_unavailable) {
|
|
ph->list.array[i]->pathPosition.deltaLongitude = lightship->pos_history[i+1]->lon - lightship->pos_history[i]->lon;
|
|
} else {
|
|
ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;
|
|
}
|
|
|
|
ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t));
|
|
*ph->list.array[i]->pathDeltaTime = (lightship->pos_history[i]->ts - lightship->pos_history[i+1]->ts)/10;
|
|
}
|
|
|
|
lightship->last_cam_lfc = now;
|
|
}
|
|
|
|
} else {
|
|
cam->cam.generationDeltaTime = now % 65536;
|
|
|
|
it2s_tender_lock_space(&facilities->epv);
|
|
it2s_tender_get_space(&facilities->epv);
|
|
bc->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude;
|
|
bc->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf;
|
|
|
|
// Set GPS coordinates
|
|
bc->referencePosition.latitude = facilities->epv.space.latitude;
|
|
bc->referencePosition.longitude = facilities->epv.space.longitude;
|
|
bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
|
|
bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
|
|
bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
|
|
it2s_tender_unlock_space(&facilities->epv);
|
|
|
|
cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency;
|
|
|
|
if (facilities->lightship->protected_zones.pz_len > 0) {
|
|
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(ProtectedCommunicationZonesRSU_t));
|
|
ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
|
|
pzs->list.count = facilities->lightship->protected_zones.pz_len;
|
|
pzs->list.size = facilities->lightship->protected_zones.pz_len * sizeof(void*);
|
|
pzs->list.array = malloc(facilities->lightship->protected_zones.pz_len * sizeof(void*));
|
|
for (int i = 0; i < facilities->lightship->protected_zones.pz_len; ++i) {
|
|
pzs->list.array[i] = calloc(1, sizeof(ProtectedCommunicationZone_t));
|
|
pzs->list.array[i]->protectedZoneLatitude = facilities->lightship->protected_zones.pz[i]->protectedZoneLatitude;
|
|
pzs->list.array[i]->protectedZoneLongitude = facilities->lightship->protected_zones.pz[i]->protectedZoneLongitude;
|
|
pzs->list.array[i]->protectedZoneType = facilities->lightship->protected_zones.pz[i]->protectedZoneType;
|
|
|
|
if (facilities->lightship->protected_zones.pz[i]->expiryTime) {
|
|
pzs->list.array[i]->expiryTime->size = facilities->lightship->protected_zones.pz[i]->expiryTime->size;
|
|
pzs->list.array[i]->expiryTime->buf = malloc(facilities->lightship->protected_zones.pz[i]->expiryTime->size);
|
|
memcpy(pzs->list.array[i]->expiryTime->buf, facilities->lightship->protected_zones.pz[i]->expiryTime->buf, facilities->lightship->protected_zones.pz[i]->expiryTime->size);
|
|
}
|
|
if (facilities->lightship->protected_zones.pz[i]->protectedZoneID) {
|
|
pzs->list.array[i]->protectedZoneID = malloc(8);
|
|
*pzs->list.array[i]->protectedZoneID = *facilities->lightship->protected_zones.pz[i]->protectedZoneID;
|
|
}
|
|
if (facilities->lightship->protected_zones.pz[i]->protectedZoneRadius) {
|
|
pzs->list.array[i]->protectedZoneRadius = malloc(8);
|
|
*pzs->list.array[i]->protectedZoneRadius = *facilities->lightship->protected_zones.pz[i]->protectedZoneRadius;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
pthread_mutex_unlock(&facilities->lightship->lock);
|
|
|
|
|
|
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_CAM, NULL, cam, cam_oer, 512);
|
|
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;
|
|
|
|
if(facilities->lightship->use_obd){
|
|
munmap(shared_message, sizeof(can_values_shm));
|
|
if(close(shm_fd) == -1)
|
|
syslog_err("[facilities] failed to close CAN shared memory\n");
|
|
}
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_CAM, cam);
|
|
|
|
return rv;
|
|
}
|
|
|
|
|
|
lightship_t* lightship_init() {
|
|
lightship_t* lightship = (lightship_t*) calloc(1, sizeof(lightship_t));
|
|
|
|
lightship->protected_zones.pz = calloc(256 , sizeof(void*));
|
|
pthread_mutex_init(&lightship->lock, NULL);
|
|
|
|
lightship->pos_history = malloc(POS_HISTORY_MAX_LEN * sizeof(void*));
|
|
|
|
int shm_fd;
|
|
|
|
shm_fd = shm_open("it2s-bluetooth-decoded", O_RDONLY, 0666);
|
|
if(shm_fd == -1){
|
|
syslog_debug("[facilities] obd shmem not found\n");
|
|
lightship->use_obd = 0;
|
|
}
|
|
else{
|
|
syslog_debug("[facilities] obd shmem found\n");
|
|
lightship->use_obd = 1;
|
|
close(shm_fd);
|
|
}
|
|
|
|
return lightship;
|
|
}
|
|
|
|
|
|
int lightship_check(lightship_t* lightship, it2s_tender_epv_t* epv) {
|
|
int rv = 0;
|
|
|
|
uint64_t now = it2s_tender_get_clock(epv);
|
|
|
|
pthread_mutex_lock(&lightship->lock);
|
|
|
|
if (lightship->type == StationType_roadSideUnit) { // RSU
|
|
if (lightship->is_vehicle_near && now > lightship->next_cam_min) {
|
|
rv = 1;
|
|
}
|
|
} else { // Vehicle
|
|
if (now > lightship->next_cam_max) {
|
|
rv = 1;
|
|
} else if (now > lightship->next_cam_min) {
|
|
|
|
it2s_tender_lock_space(epv);
|
|
it2s_tender_get_space(epv);
|
|
|
|
// Check heading delta > 4º
|
|
int diff = epv->space.heading - lightship->pos_history[0]->heading;
|
|
if (abs(diff) > 40) rv = 1;
|
|
|
|
if (!rv) {
|
|
// Check speed delta > 0.5 m/s
|
|
diff = epv->space.speed - lightship->pos_history[0]->speed;
|
|
if (abs(diff) > 50) rv = 1;
|
|
|
|
if (!rv) {
|
|
// Check position delta > 4 m
|
|
// TODO make an *accurate* distance calculator using GPS coords
|
|
int32_t avg_speed = (epv->space.speed + lightship->pos_history[0]->speed)/2 / 100; /* cm/s to m/s */
|
|
uint64_t delta_time = (now - lightship->last_cam) / 1000; /* ms to s */
|
|
if (avg_speed * delta_time > 4) rv = 1;
|
|
}
|
|
}
|
|
it2s_tender_unlock_space(epv);
|
|
}
|
|
|
|
|
|
// Remove expired PZs
|
|
for (int i = 0; i < lightship->protected_zones.pz_len; ++i) {
|
|
uint64_t expiry;
|
|
if (lightship->protected_zones.pz[i]->expiryTime) {
|
|
asn_INTEGER2ulong(lightship->protected_zones.pz[i]->expiryTime, &expiry);
|
|
if (now >= expiry) {
|
|
ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, lightship->protected_zones.pz[i]);
|
|
for (int j = i; j < lightship->protected_zones.pz_len - 1; ++j) {
|
|
lightship->protected_zones.pz[j] = lightship->protected_zones.pz[j+1];
|
|
}
|
|
--lightship->protected_zones.pz_len;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
pthread_mutex_unlock(&lightship->lock);
|
|
|
|
return rv;
|
|
}
|
|
|
|
void lightship_reset_timer(lightship_t* lightship, it2s_tender_epv_t* epv) {
|
|
uint64_t now = it2s_tender_get_clock(epv);
|
|
|
|
pthread_mutex_lock(&lightship->lock);
|
|
|
|
if (lightship->type != StationType_roadSideUnit) { // Vehicle
|
|
lightship->next_cam_max = now + lightship->vehicle_gen_max;
|
|
lightship->next_cam_min = now + lightship->vehicle_gen_min;
|
|
} else { // RSU
|
|
if (now > lightship->last_vehicle + lightship->rsu_vehicle_permanence) {
|
|
lightship->is_vehicle_near = false;
|
|
}
|
|
lightship->next_cam_min = now + lightship->rsu_gen_min;
|
|
}
|
|
|
|
pthread_mutex_unlock(&lightship->lock);
|
|
}
|
|
|
|
enum CAM_CHECK_R check_cam(void* fc, BTPPacketIndication_t *bpi, CAM_t* cam, it2s_tender_epv_t* epv, uint8_t* ssp, uint32_t ssp_len) {
|
|
int rv = 0;
|
|
facilities_t* facilities = (facilities_t*) fc;
|
|
lightship_t *lightship = ((facilities_t*) fc)->lightship;
|
|
|
|
uint64_t now = it2s_tender_get_clock(epv);
|
|
|
|
// Check permissions
|
|
if (ssp) {
|
|
if (cam->cam.camParameters.highFrequencyContainer.present == HighFrequencyContainer_PR_rsuContainerHighFrequency &&
|
|
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
|
|
if (!permissions_check(CID_PROTECTED_ZONES, ssp, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PROTECTED_ZONES].container);
|
|
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, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PUBLIC_TRANSPORT].container);
|
|
return rv;
|
|
}
|
|
break;
|
|
case SpecialVehicleContainer_PR_specialTransportContainer:
|
|
if (!permissions_check(CID_SPECIAL_TRANSPORT, ssp, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SPECIAL_TRANSPORT].container);
|
|
return rv;
|
|
}
|
|
break;
|
|
case SpecialVehicleContainer_PR_dangerousGoodsContainer:
|
|
if (!permissions_check(CID_DANGEROUS_GOODS, ssp, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_DANGEROUS_GOODS].container);
|
|
return rv;
|
|
}
|
|
break;
|
|
case SpecialVehicleContainer_PR_roadWorksContainerBasic:
|
|
if (!permissions_check(CID_ROADWORK, ssp, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_ROADWORK].container);
|
|
return rv;
|
|
}
|
|
if (cam->cam.camParameters.specialVehicleContainer->choice.roadWorksContainerBasic.closedLanes) {
|
|
if (!permissions_check(CID_CLOSED_LANES, ssp, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_CLOSED_LANES].container);
|
|
return rv;
|
|
}
|
|
}
|
|
break;
|
|
case SpecialVehicleContainer_PR_rescueContainer:
|
|
if (!permissions_check(CID_RESCUE, ssp, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_RESCUE].container);
|
|
return rv;
|
|
}
|
|
break;
|
|
case SpecialVehicleContainer_PR_emergencyContainer:
|
|
if (!permissions_check(CID_EMERGENCY, ssp, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_EMERGENCY].container);
|
|
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, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_REQUEST_FOR_RIGHT_OF_WAY].container);
|
|
return rv;
|
|
}
|
|
}
|
|
if (bm & 0x01) {
|
|
if (!permissions_check(CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, ssp, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT].container);
|
|
return rv;
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
case SpecialVehicleContainer_PR_safetyCarContainer:
|
|
if (!permissions_check(CID_SAFETY_CAR, ssp, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SAFETY_CAR].container);
|
|
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, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING].container);
|
|
return rv;
|
|
}
|
|
break;
|
|
case TrafficRule_noPassingForTrucks:
|
|
if (!permissions_check(CID_NO_PASSING_FOR_TRUCKS, ssp, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING_FOR_TRUCKS].container);
|
|
return rv;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.speedLimit) {
|
|
if (!permissions_check(CID_SPEED_LIMIT, ssp, ssp_len)) {
|
|
rv = CAM_BAD_PERMISSIONS;
|
|
syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SPEED_LIMIT].container);
|
|
return rv;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
pthread_mutex_lock(&lightship->lock);
|
|
if (lightship->type == StationType_roadSideUnit) {
|
|
// Send CAMs if vehicles nearby
|
|
if (bpi->stationType != StationType_roadSideUnit && bpi->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->protected_zones.pz_len < 255) {
|
|
|
|
bool new_pz = false;
|
|
for (int k = 0; k < pzs->list.count; ++k) {
|
|
|
|
bool found = false;
|
|
for (int j = 0; j < lightship->protected_zones.pz_len; ++j) {
|
|
if (lightship->protected_zones.pz[j]->protectedZoneLatitude == pzs->list.array[k]->protectedZoneLatitude &&
|
|
lightship->protected_zones.pz[j]->protectedZoneLongitude == pzs->list.array[k]->protectedZoneLongitude) {
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
if (found) continue;
|
|
|
|
new_pz = true;
|
|
|
|
lightship->protected_zones.pz[lightship->protected_zones.pz_len] = calloc(1, sizeof(ProtectedCommunicationZone_t));
|
|
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLatitude = pzs->list.array[k]->protectedZoneLatitude;
|
|
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLongitude = pzs->list.array[k]->protectedZoneLongitude;
|
|
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneType = pzs->list.array[k]->protectedZoneType;
|
|
|
|
if (pzs->list.array[k]->expiryTime) {
|
|
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->size = pzs->list.array[k]->expiryTime->size;
|
|
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf = malloc(pzs->list.array[k]->expiryTime->size);
|
|
memcpy(lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf, pzs->list.array[k]->expiryTime->buf, pzs->list.array[k]->expiryTime->size);
|
|
}
|
|
if (pzs->list.array[k]->protectedZoneID) {
|
|
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = malloc(8);
|
|
*lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = *pzs->list.array[k]->protectedZoneID;
|
|
}
|
|
if (pzs->list.array[k]->protectedZoneRadius) {
|
|
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = malloc(8);
|
|
*lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = *pzs->list.array[k]->protectedZoneRadius;
|
|
}
|
|
++lightship->protected_zones.pz_len;
|
|
}
|
|
|
|
// Inform [management]
|
|
if (new_pz) {
|
|
uint8_t b_oer[512];
|
|
ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t));
|
|
mreq->present = ManagementRequest_PR_attributes;
|
|
mreq->choice.attributes.present = ManagementRequestAttributes_PR_set;
|
|
mreq->choice.attributes.choice.set.protectedZones = calloc(1, sizeof(struct protectedZones));
|
|
mreq->choice.attributes.choice.set.protectedZones->list.count = lightship->protected_zones.pz_len;
|
|
mreq->choice.attributes.choice.set.protectedZones->list.size = lightship->protected_zones.pz_len * sizeof(void*);
|
|
mreq->choice.attributes.choice.set.protectedZones->list.array = calloc(lightship->protected_zones.pz_len, sizeof(void*));
|
|
for (int p = 0; p < lightship->protected_zones.pz_len; ++p) {
|
|
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ProtectedCommunicationZone, NULL, lightship->protected_zones.pz[p], b_oer, 512);
|
|
oer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void**) &mreq->choice.attributes.choice.set.protectedZones->list.array[p], b_oer, enc.encoded);
|
|
}
|
|
|
|
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ManagementRequest, NULL, mreq, b_oer, 512);
|
|
void* management_socket = zmq_socket(facilities->zmq.ctx, ZMQ_REQ);
|
|
int wait_ms = 1000;
|
|
zmq_setsockopt(management_socket, ZMQ_RCVTIMEO, &wait_ms, sizeof(int));
|
|
zmq_connect(management_socket, facilities->zmq.management_address);
|
|
zmq_send(management_socket, b_oer, enc.encoded, 0);
|
|
syslog_debug("[facilities] [ca]-> sending MReq.attributes.set.protectedZones to ->[management]");
|
|
uint8_t code;
|
|
zmq_recv(management_socket, &code, 1, 0);
|
|
zmq_close(management_socket);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
pthread_mutex_unlock(&lightship->lock);
|
|
|
|
return rv;
|
|
}
|
|
|
|
static int check_pz(lightship_t *lightship, it2s_tender_epv_t* epv) {
|
|
bool is_inside = false;
|
|
|
|
it2s_tender_lock_space(epv);
|
|
it2s_tender_get_space(epv);
|
|
double lat = epv->space.latitude/10000000.0;
|
|
double lon = epv->space.longitude/10000000.0;
|
|
it2s_tender_unlock_space(epv);
|
|
|
|
pthread_mutex_lock(&lightship->lock);
|
|
|
|
for (int i = 0; i < lightship->protected_zones.pz_len; ++i) {
|
|
double d = haversine(lat, lon, (double) lightship->protected_zones.pz[i]->protectedZoneLatitude/10000000.0, (double) lightship->protected_zones.pz[i]->protectedZoneLongitude/10000000.0);
|
|
|
|
int pz_radius = 50;
|
|
if (lightship->protected_zones.pz[i]->protectedZoneRadius) {
|
|
pz_radius = *lightship->protected_zones.pz[i]->protectedZoneRadius;
|
|
}
|
|
if (d < pz_radius) {
|
|
is_inside = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
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;
|
|
|
|
TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t));
|
|
tr->present = TransportRequest_PR_packet;
|
|
TransportPacketRequest_t* tpr = &tr->choice.packet;
|
|
tpr->present = TransportPacketRequest_PR_btp;
|
|
BTPPacketRequest_t *bpr = &tpr->choice.btp;
|
|
|
|
bpr->btpType = BTPType_btpB;
|
|
|
|
bpr->gn.destinationAddress.buf = malloc(6);
|
|
for (int i = 0; i < 6; ++i) {
|
|
bpr->gn.destinationAddress.buf[i] = 0xff;
|
|
}
|
|
bpr->gn.destinationAddress.size = 6;
|
|
|
|
bpr->gn.packetTransportType = PacketTransportType_shb;
|
|
|
|
bpr->destinationPort = Port_cam;
|
|
|
|
bpr->gn.trafficClass = 2;
|
|
|
|
bpr->data.buf = malloc(512);
|
|
|
|
// Fill header for FacilitiesIndication and FacilitiesMessageIndication structs
|
|
|
|
bpr->gn.securityProfile.sign = true;
|
|
|
|
FacilitiesIndication_t* fi = calloc(1,sizeof(FacilitiesIndication_t));
|
|
fi->present = FacilitiesIndication_PR_message;
|
|
FacilitiesMessageIndication_t* fmi = &fi->choice.message;
|
|
fmi->itsMessageType = ItsMessageType_cam;
|
|
fmi->data.buf = malloc(512);
|
|
|
|
uint8_t tr_oer[1024];
|
|
uint8_t fi_oer[1024];
|
|
tr_oer[0] = 4; // Facilities
|
|
fi_oer[0] = 4;
|
|
while (!facilities->exit) {
|
|
usleep(1000*50);
|
|
|
|
if (lightship_check(facilities->lightship, &facilities->epv) && facilities->lightship->active) {
|
|
rv = mk_cam(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size);
|
|
if (rv) {
|
|
continue;
|
|
}
|
|
memcpy(fmi->data.buf, bpr->data.buf, bpr->data.size);
|
|
fmi->data.size = bpr->data.size;
|
|
|
|
// Check if inside PZ
|
|
bpr->gn.communicationProfile = 0;
|
|
if (facilities->station_type != 15 && check_pz(facilities->lightship, &facilities->epv)) bpr->gn.communicationProfile = 1;
|
|
|
|
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 1023);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] encoding TR for cam failed");
|
|
continue;
|
|
}
|
|
|
|
asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, fi_oer+1, 1023);
|
|
if(enc_fdi.encoded == -1){
|
|
syslog_err("[facilities] encoding FI for cam failed");
|
|
continue;
|
|
}
|
|
|
|
queue_add(facilities->tx_queue, tr_oer, enc.encoded+1, 3);
|
|
pthread_cond_signal(&facilities->tx_queue->trigger);
|
|
|
|
queue_add(facilities->tx_queue, fi_oer, enc_fdi.encoded+1, 5);
|
|
pthread_cond_signal(&facilities->tx_queue->trigger);
|
|
|
|
lightship_reset_timer(facilities->lightship, &facilities->epv);
|
|
}
|
|
|
|
}
|
|
|
|
ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
|
|
|
|
return NULL;
|
|
}
|