866 lines
40 KiB
C
866 lines
40 KiB
C
#include "cam.h"
|
||
#include "facilities.h"
|
||
|
||
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
|
||
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
|
||
#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementRequest.h>
|
||
#include <it2s-asn/etsi-its-v1/cam/EI1_CAM.h>
|
||
#include <it2s-asn/etsi-its-v2/cdd-2.2.1/EI2_StationType.h>
|
||
#include <it2s-asn/etsi-its-v2/cam/EI2_CAM.h>
|
||
|
||
#include <sys/mman.h>
|
||
#include <stdint.h>
|
||
#include <time.h>
|
||
#include <zmq.h>
|
||
#include <unistd.h>
|
||
#include <math.h>
|
||
#include <fcntl.h>
|
||
|
||
#include <it2s-tender/space.h>
|
||
#include <it2s-tender/time.h>
|
||
#include <it2s-tender/database.h>
|
||
#include <it2s-tender/constants.h>
|
||
#include <it2s-tender/recorder.h>
|
||
#include <it2s-tender/packet.h>
|
||
|
||
#define EARTH_RADIUS 6369000.0
|
||
#define RAD_PER_DEG M_PI/180.0
|
||
|
||
// Using Vanetza's Path History constraints
|
||
#define PATH_HISTORY_ALLOWABLE_ERROR 0.47
|
||
#define PATH_HISTORY_CHORD_LENGTH_THRESHOLD 22.5
|
||
#define PATH_HISTORY_SMALL_DELTA_PHI 1.0
|
||
#define PATH_HISTORY_DISTANCE 200.0
|
||
#define PATH_HISTORY_MAX_ESTIMATED_RADIUS EARTH_RADIUS
|
||
|
||
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) {
|
||
log_debug("[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;
|
||
|
||
}
|
||
|
||
/*
|
||
* Implementation of Path History as per the report,
|
||
* "Vehicle Safety Communications – Applications (VSC-A) Final Report: Appendix Volume 1"
|
||
* "Appendix B-2 - Method One (Section 3.2)"
|
||
*/
|
||
static void path_history_update(pos_point_t* pn) {
|
||
|
||
lightship_t* ls = &facilities.lightship;
|
||
|
||
++ls->concise_points_len;
|
||
if (ls->concise_points_len > 3) {
|
||
ls->concise_points_len = 3;
|
||
}
|
||
for (int i = ls->concise_points_len - (1 + ls->concise_keep_start); i > 0; --i) {
|
||
ls->concise_points[i] = ls->concise_points[i-1];
|
||
}
|
||
ls->concise_points[0] = *pn;
|
||
|
||
if (ls->concise_points_len == 3) {
|
||
double actual_error;
|
||
double actual_chord_length = it2s_geodesy_haversine(
|
||
ls->concise_points[2].lat / 1.0e7,
|
||
ls->concise_points[2].lon / 1.0e7,
|
||
ls->concise_points[0].lat / 1.0e7,
|
||
ls->concise_points[0].lon / 1.0e7
|
||
);
|
||
if (actual_chord_length > PATH_HISTORY_CHORD_LENGTH_THRESHOLD) {
|
||
actual_error = PATH_HISTORY_ALLOWABLE_ERROR + 1.0;
|
||
} else {
|
||
double dtheta = ((int)ls->concise_points[0].heading - ls->concise_points[2].heading) / 10.0;
|
||
double estimated_r;
|
||
if (dtheta < PATH_HISTORY_SMALL_DELTA_PHI) {
|
||
actual_error = 0;
|
||
estimated_r = PATH_HISTORY_MAX_ESTIMATED_RADIUS;
|
||
} else {
|
||
estimated_r = actual_chord_length/(2*sin(RAD_PER_DEG * dtheta/2));
|
||
double d = estimated_r * cos(RAD_PER_DEG * dtheta/2);
|
||
actual_error = estimated_r - d;
|
||
}
|
||
|
||
}
|
||
if (actual_error > PATH_HISTORY_ALLOWABLE_ERROR) { /* add to path history */
|
||
if (ls->path_history_len != PATH_HISTORY_MAX_LEN) {
|
||
++ls->path_history_len;
|
||
} else { /* Path History is full */
|
||
free(ls->path_history[PATH_HISTORY_MAX_LEN - 1]);
|
||
}
|
||
|
||
for (int i = ls->path_history_len-1; i > 0; --i) {
|
||
ls->path_history[i] = ls->path_history[i-1];
|
||
}
|
||
ls->path_history[0] = malloc(sizeof(pos_point_t));
|
||
*ls->path_history[0] = ls->concise_points[1];
|
||
|
||
ls->concise_keep_start = false;
|
||
} else {
|
||
ls->concise_keep_start = true;
|
||
}
|
||
}
|
||
|
||
if (ls->path_history_len >= 2) {
|
||
double distance = 0.0;
|
||
for (int i = 0; i < ls->path_history_len - 1; ++i) {
|
||
distance += it2s_geodesy_haversine(
|
||
ls->path_history[i]->lat / 1.0e7,
|
||
ls->path_history[i]->lon / 1.0e7,
|
||
ls->path_history[i+1]->lat / 1.0e7,
|
||
ls->path_history[i+1]->lon / 1.0e7
|
||
);
|
||
if (distance > PATH_HISTORY_DISTANCE) {
|
||
for (int j = i; j < ls->path_history_len - 1; ++j) {
|
||
free(ls->path_history[j+1]);
|
||
}
|
||
ls->path_history_len = i + 1;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
static int mk_cam(uint8_t *cam_oer, uint16_t *cam_len) {
|
||
int rv = 0;
|
||
int shm_fd, shm_valid = 0;
|
||
|
||
lightship_t* lightship = &facilities.lightship;
|
||
|
||
EI2_CAM_t *cam = calloc(1, sizeof(EI2_CAM_t));
|
||
|
||
cam->header.protocolVersion = 2;
|
||
cam->header.messageId = EI2_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;
|
||
|
||
EI2_BasicContainer_t* bc = &cam->cam.camParameters.basicContainer;
|
||
|
||
uint64_t now = itss_time_get();
|
||
|
||
pthread_mutex_lock(&lightship->lock);
|
||
|
||
if (facilities.station_type != EI2_StationType_roadSideUnit) {
|
||
cam->cam.generationDeltaTime = now % 65536;
|
||
|
||
itss_space_lock();
|
||
itss_space_get();
|
||
bc->referencePosition.altitude.altitudeValue = epv.space.data.altitude.value;
|
||
bc->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
|
||
|
||
// Set GPS coordinates
|
||
bc->referencePosition.latitude = epv.space.data.latitude.value;
|
||
bc->referencePosition.longitude = epv.space.data.longitude.value;
|
||
uint16_t lat_conf = epv.space.data.latitude.confidence;
|
||
uint16_t lon_conf = epv.space.data.longitude.confidence;
|
||
|
||
cam->cam.camParameters.highFrequencyContainer.present = EI2_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
|
||
EI2_BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
|
||
|
||
// Set speed
|
||
bvc_hf->speed.speedValue = epv.space.data.speed.value;
|
||
bvc_hf->speed.speedConfidence = epv.space.data.speed.confidence;
|
||
|
||
|
||
// Set heading
|
||
bvc_hf->heading.headingValue = epv.space.data.heading.value;
|
||
bvc_hf->heading.headingConfidence = epv.space.data.heading.confidence;
|
||
|
||
itss_space_unlock(epv);
|
||
|
||
if (lat_conf > lon_conf) {
|
||
bc->referencePosition.positionConfidenceEllipse.semiMinorAxisLength = lon_conf;
|
||
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisLength = lat_conf;
|
||
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = 0;
|
||
} else {
|
||
bc->referencePosition.positionConfidenceEllipse.semiMinorAxisLength = lon_conf;
|
||
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisLength = lat_conf;
|
||
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = 900;
|
||
}
|
||
|
||
bvc_hf->vehicleWidth = facilities.vehicle.width;
|
||
bvc_hf->vehicleLength.vehicleLengthValue = facilities.vehicle.length;
|
||
bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = EI2_VehicleLengthConfidenceIndication_unavailable;
|
||
bvc_hf->longitudinalAcceleration.value = EI2_AccelerationValue_unavailable;
|
||
bvc_hf->longitudinalAcceleration.confidence = EI2_AccelerationConfidence_unavailable;
|
||
|
||
bvc_hf->driveDirection = EI2_DriveDirection_unavailable;
|
||
bvc_hf->curvature.curvatureValue = EI2_CurvatureValue_unavailable;
|
||
bvc_hf->curvature.curvatureConfidence = EI2_CurvatureConfidence_unavailable;
|
||
bvc_hf->yawRate.yawRateValue = EI2_YawRateValue_unavailable;
|
||
bvc_hf->yawRate.yawRateConfidence = EI2_YawRateConfidence_unavailable;
|
||
|
||
/*
|
||
// Save current values
|
||
if (lightship->path_history_len == POS_HISTORY_MAX_LEN) {
|
||
free(lightship->path_history[POS_HISTORY_MAX_LEN-1]);
|
||
--lightship->path_history_len;
|
||
}
|
||
*/
|
||
|
||
/*
|
||
for (int i = lightship->path_history_len-1; i >= 0; --i) {
|
||
lightship->path_history[i+1] = lightship->path_history[i];
|
||
}
|
||
*/
|
||
|
||
pos_point_t pn;
|
||
pn.heading = bvc_hf->heading.headingValue;
|
||
pn.lat = bc->referencePosition.latitude;
|
||
pn.lon = bc->referencePosition.longitude;
|
||
pn.alt = bc->referencePosition.altitude.altitudeValue;
|
||
pn.speed = bvc_hf->speed.speedValue;
|
||
pn.ts = now;
|
||
|
||
path_history_update(&pn);
|
||
|
||
lightship->t_last_cam = now;
|
||
|
||
// Acceleration
|
||
if (lightship->last_pos.ts != 0) {
|
||
double delta_angle = ((int)pn.heading - lightship->last_pos.heading) / 10.0; /* 1º */
|
||
double delta_speed = (pn.speed - (lightship->last_pos.speed * cos(delta_angle * RAD_PER_DEG))) / 10.0; /* 0.1 m/s */
|
||
int16_t long_a = (int16_t) (delta_speed / (double) ((pn.ts - lightship->last_pos.ts) / 1000.0)); /* 0.1 m/s^2 */
|
||
|
||
if (long_a > 160) long_a = 160;
|
||
else if (long_a < -160) long_a = -160;
|
||
|
||
bvc_hf->longitudinalAcceleration.value = long_a;
|
||
bvc_hf->longitudinalAcceleration.confidence = EI2_AccelerationConfidence_unavailable;
|
||
} else {
|
||
bvc_hf->longitudinalAcceleration.value = EI2_AccelerationValue_unavailable;
|
||
bvc_hf->longitudinalAcceleration.confidence = EI2_AccelerationConfidence_unavailable;
|
||
}
|
||
|
||
// Low frequency container
|
||
if (now > lightship->t_last_cam_lfc + 500) {
|
||
cam->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(EI2_LowFrequencyContainer_t));
|
||
|
||
cam->cam.camParameters.lowFrequencyContainer->present = EI2_LowFrequencyContainer_PR_basicVehicleContainerLowFrequency;
|
||
|
||
EI2_BasicVehicleContainerLowFrequency_t* bvc_lf = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency;
|
||
|
||
EI2_Path_t* ph = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory;
|
||
|
||
if (lightship->path_history_len != 0) {
|
||
ph->list.array = malloc((lightship->path_history_len) * sizeof(void*));
|
||
ph->list.count = lightship->path_history_len;
|
||
ph->list.size = (lightship->path_history_len) * sizeof(void*);
|
||
|
||
ph->list.array[0] = calloc(1,sizeof(EI2_PathPoint_t));
|
||
|
||
if (lightship->path_history[0]->alt != EI2_AltitudeValue_unavailable && pn.alt != EI2_AltitudeValue_unavailable) {
|
||
ph->list.array[0]->pathPosition.deltaAltitude = lightship->path_history[0]->alt - pn.alt;
|
||
} else {
|
||
ph->list.array[0]->pathPosition.deltaAltitude = EI2_DeltaAltitude_unavailable;
|
||
}
|
||
|
||
if (lightship->path_history[0]->lat != EI2_Latitude_unavailable && pn.lat != EI2_Latitude_unavailable) {
|
||
ph->list.array[0]->pathPosition.deltaLatitude = lightship->path_history[0]->lat - pn.lat;
|
||
} else {
|
||
ph->list.array[0]->pathPosition.deltaLatitude = EI2_DeltaLatitude_unavailable;
|
||
}
|
||
|
||
if (lightship->path_history[0]->lon != EI2_Longitude_unavailable && pn.lon != EI2_Longitude_unavailable) {
|
||
ph->list.array[0]->pathPosition.deltaLongitude = lightship->path_history[0]->lon - pn.lon;
|
||
} else {
|
||
ph->list.array[0]->pathPosition.deltaLongitude = EI2_DeltaLongitude_unavailable;
|
||
}
|
||
|
||
ph->list.array[0]->pathDeltaTime = calloc(1,sizeof(EI2_PathDeltaTime_t));
|
||
*ph->list.array[0]->pathDeltaTime = (pn.ts - lightship->path_history[0]->ts)/10;
|
||
|
||
for (int i = 1; i < lightship->path_history_len; ++i) {
|
||
ph->list.array[i] = calloc(1,sizeof(EI2_PathPoint_t));
|
||
|
||
if (lightship->path_history[i]->alt != EI2_AltitudeValue_unavailable && lightship->path_history[i-1]->alt != EI2_AltitudeValue_unavailable) {
|
||
ph->list.array[i]->pathPosition.deltaAltitude = lightship->path_history[i]->alt - lightship->path_history[i-1]->alt;
|
||
} else {
|
||
ph->list.array[i]->pathPosition.deltaAltitude = EI2_DeltaAltitude_unavailable;
|
||
}
|
||
|
||
if (lightship->path_history[i]->lat != EI2_Latitude_unavailable && lightship->path_history[i-1]->lat != EI2_Latitude_unavailable) {
|
||
ph->list.array[i]->pathPosition.deltaLatitude = lightship->path_history[i]->lat - lightship->path_history[i-1]->lat;
|
||
} else {
|
||
ph->list.array[i]->pathPosition.deltaLatitude = EI2_DeltaLatitude_unavailable;
|
||
}
|
||
|
||
if (lightship->path_history[i]->lon != EI2_Longitude_unavailable && lightship->path_history[i-1]->lon != EI2_Longitude_unavailable) {
|
||
ph->list.array[i]->pathPosition.deltaLongitude = lightship->path_history[i]->lon - lightship->path_history[i-1]->lon;
|
||
} else {
|
||
ph->list.array[i]->pathPosition.deltaLongitude = EI2_DeltaLongitude_unavailable;
|
||
}
|
||
|
||
ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(EI2_PathDeltaTime_t));
|
||
*ph->list.array[i]->pathDeltaTime = (lightship->path_history[i-1]->ts - lightship->path_history[i]->ts)/10;
|
||
}
|
||
}
|
||
|
||
lightship->t_last_cam_lfc = now;
|
||
}
|
||
|
||
lightship->last_pos = pn;
|
||
|
||
} else {
|
||
cam->cam.generationDeltaTime = now % 65536;
|
||
|
||
itss_space_lock();
|
||
itss_space_get();
|
||
bc->referencePosition.altitude.altitudeValue = epv.space.data.altitude.value;
|
||
bc->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
|
||
|
||
// Set GPS coordinates
|
||
bc->referencePosition.latitude = epv.space.data.latitude.value;
|
||
bc->referencePosition.longitude = epv.space.data.longitude.value;
|
||
bc->referencePosition.positionConfidenceEllipse.semiMinorAxisLength = EI2_SemiAxisLength_unavailable;
|
||
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisLength = EI2_SemiAxisLength_unavailable;
|
||
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = EI2_HeadingValue_unavailable;
|
||
itss_space_unlock();
|
||
|
||
cam->cam.camParameters.highFrequencyContainer.present = EI2_HighFrequencyContainer_PR_rsuContainerHighFrequency;
|
||
|
||
if (lightship->protected_zones.pz_len > 0) {
|
||
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(EI2_ProtectedCommunicationZonesRSU_t));
|
||
EI2_ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
|
||
pzs->list.count = lightship->protected_zones.pz_len;
|
||
pzs->list.size = lightship->protected_zones.pz_len * sizeof(void*);
|
||
pzs->list.array = malloc(lightship->protected_zones.pz_len * sizeof(void*));
|
||
for (int i = 0; i < lightship->protected_zones.pz_len; ++i) {
|
||
pzs->list.array[i] = calloc(1, sizeof(EI2_ProtectedCommunicationZone_t));
|
||
pzs->list.array[i]->protectedZoneLatitude = lightship->protected_zones.pz[i]->protectedZoneLatitude;
|
||
pzs->list.array[i]->protectedZoneLongitude = lightship->protected_zones.pz[i]->protectedZoneLongitude;
|
||
pzs->list.array[i]->protectedZoneType = lightship->protected_zones.pz[i]->protectedZoneType;
|
||
|
||
if (lightship->protected_zones.pz[i]->expiryTime) {
|
||
pzs->list.array[i]->expiryTime->size = lightship->protected_zones.pz[i]->expiryTime->size;
|
||
pzs->list.array[i]->expiryTime->buf = malloc(lightship->protected_zones.pz[i]->expiryTime->size);
|
||
memcpy(pzs->list.array[i]->expiryTime->buf, lightship->protected_zones.pz[i]->expiryTime->buf, lightship->protected_zones.pz[i]->expiryTime->size);
|
||
}
|
||
if (lightship->protected_zones.pz[i]->protectedZoneID) {
|
||
pzs->list.array[i]->protectedZoneId = malloc(8);
|
||
*pzs->list.array[i]->protectedZoneId = *lightship->protected_zones.pz[i]->protectedZoneID;
|
||
}
|
||
if (lightship->protected_zones.pz[i]->protectedZoneRadius) {
|
||
pzs->list.array[i]->protectedZoneRadius = malloc(8);
|
||
*pzs->list.array[i]->protectedZoneRadius = *lightship->protected_zones.pz[i]->protectedZoneRadius;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
pthread_mutex_unlock(&lightship->lock);
|
||
|
||
|
||
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EI2_CAM, NULL, cam, cam_oer, 512);
|
||
if (enc.encoded == -1) {
|
||
log_error("[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_EI2_CAM, cam);
|
||
|
||
return rv;
|
||
}
|
||
|
||
|
||
int lightship_init() {
|
||
|
||
lightship_t* lightship = &facilities.lightship;
|
||
|
||
lightship->protected_zones.pz = calloc(256, sizeof(void*));
|
||
pthread_mutex_init(&lightship->lock, NULL);
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
int lightship_check() {
|
||
int rv = 0;
|
||
|
||
lightship_t* lightship = &facilities.lightship;
|
||
|
||
uint64_t now = itss_time_get();
|
||
|
||
pthread_mutex_lock(&lightship->lock);
|
||
|
||
if (lightship->type == EI1_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) {
|
||
|
||
itss_space_lock();
|
||
itss_space_get();
|
||
|
||
// Check heading delta > 4º
|
||
int diff = epv.space.data.heading.value - lightship->last_pos.heading;
|
||
diff += (diff > 1800) ? -3600 : (diff < -1800) ? 3600 : 0;
|
||
if (abs(diff) > 40) rv = 1;
|
||
|
||
if (!rv) {
|
||
// Check speed delta > 0.5 m/s
|
||
diff = epv.space.data.speed.value - lightship->last_pos.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.data.speed.value + lightship->last_pos.speed)/2 / 100; /* cm/s to m/s */
|
||
uint64_t delta_time = (now - lightship->t_last_cam) / 1000; /* ms to s */
|
||
if (avg_speed * delta_time > 4) rv = 1;
|
||
}
|
||
}
|
||
itss_space_unlock(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_EI1_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 = &facilities.lightship;
|
||
|
||
uint64_t now = itss_time_get();
|
||
|
||
pthread_mutex_lock(&lightship->lock);
|
||
|
||
if (lightship->type != EI1_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->t_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(EIS_BTPPacketIndication_t *bpi, void* cam, uint8_t* ssp, uint32_t ssp_len) {
|
||
int rv = 0;
|
||
lightship_t* lightship = &facilities.lightship;
|
||
|
||
uint64_t now = itss_time_get();
|
||
|
||
/* CAM 1.4.1 and 2.1.1 are the same */
|
||
EI2_CAM_t* cam_c = cam;
|
||
|
||
// Check permissions
|
||
if (ssp) {
|
||
if (cam_c->cam.camParameters.highFrequencyContainer.present == EI2_HighFrequencyContainer_PR_rsuContainerHighFrequency &&
|
||
cam_c->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
|
||
if (!permissions_check(CID_PROTECTED_ZONES, ssp, ssp_len)) {
|
||
rv = CAM_BAD_PERMISSIONS;
|
||
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PROTECTED_ZONES].container);
|
||
return rv;
|
||
}
|
||
}
|
||
if (cam_c->cam.camParameters.specialVehicleContainer) {
|
||
switch (cam_c->cam.camParameters.specialVehicleContainer->present) {
|
||
case EI2_SpecialVehicleContainer_PR_NOTHING:
|
||
break;
|
||
case EI2_SpecialVehicleContainer_PR_publicTransportContainer:
|
||
if (!permissions_check(CID_PUBLIC_TRANSPORT, ssp, ssp_len)) {
|
||
rv = CAM_BAD_PERMISSIONS;
|
||
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PUBLIC_TRANSPORT].container);
|
||
return rv;
|
||
}
|
||
break;
|
||
case EI2_SpecialVehicleContainer_PR_specialTransportContainer:
|
||
if (!permissions_check(CID_SPECIAL_TRANSPORT, ssp, ssp_len)) {
|
||
rv = CAM_BAD_PERMISSIONS;
|
||
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SPECIAL_TRANSPORT].container);
|
||
return rv;
|
||
}
|
||
break;
|
||
case EI2_SpecialVehicleContainer_PR_dangerousGoodsContainer:
|
||
if (!permissions_check(CID_DANGEROUS_GOODS, ssp, ssp_len)) {
|
||
rv = CAM_BAD_PERMISSIONS;
|
||
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_DANGEROUS_GOODS].container);
|
||
return rv;
|
||
}
|
||
break;
|
||
case EI2_SpecialVehicleContainer_PR_roadWorksContainerBasic:
|
||
if (!permissions_check(CID_ROADWORK, ssp, ssp_len)) {
|
||
rv = CAM_BAD_PERMISSIONS;
|
||
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_ROADWORK].container);
|
||
return rv;
|
||
}
|
||
if (cam_c->cam.camParameters.specialVehicleContainer->choice.roadWorksContainerBasic.closedLanes) {
|
||
if (!permissions_check(CID_CLOSED_LANES, ssp, ssp_len)) {
|
||
rv = CAM_BAD_PERMISSIONS;
|
||
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_CLOSED_LANES].container);
|
||
return rv;
|
||
}
|
||
}
|
||
break;
|
||
case EI2_SpecialVehicleContainer_PR_rescueContainer:
|
||
if (!permissions_check(CID_RESCUE, ssp, ssp_len)) {
|
||
rv = CAM_BAD_PERMISSIONS;
|
||
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_RESCUE].container);
|
||
return rv;
|
||
}
|
||
break;
|
||
case EI2_SpecialVehicleContainer_PR_emergencyContainer:
|
||
if (!permissions_check(CID_EMERGENCY, ssp, ssp_len)) {
|
||
rv = CAM_BAD_PERMISSIONS;
|
||
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_EMERGENCY].container);
|
||
return rv;
|
||
}
|
||
if (cam_c->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority &&
|
||
cam_c->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf) {
|
||
// TODO verify bitmap
|
||
uint8_t bm = *cam_c->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;
|
||
log_debug("[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;
|
||
log_debug("[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 EI2_SpecialVehicleContainer_PR_safetyCarContainer:
|
||
if (!permissions_check(CID_SAFETY_CAR, ssp, ssp_len)) {
|
||
rv = CAM_BAD_PERMISSIONS;
|
||
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SAFETY_CAR].container);
|
||
return rv;
|
||
}
|
||
if (cam_c->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) {
|
||
switch (*cam_c->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) {
|
||
case EI2_TrafficRule_noPassing:
|
||
if (!permissions_check(CID_NO_PASSING, ssp, ssp_len)) {
|
||
rv = CAM_BAD_PERMISSIONS;
|
||
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING].container);
|
||
return rv;
|
||
}
|
||
break;
|
||
case EI2_TrafficRule_noPassingForTrucks:
|
||
if (!permissions_check(CID_NO_PASSING_FOR_TRUCKS, ssp, ssp_len)) {
|
||
rv = CAM_BAD_PERMISSIONS;
|
||
log_debug("[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_c->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.speedLimit) {
|
||
if (!permissions_check(CID_SPEED_LIMIT, ssp, ssp_len)) {
|
||
rv = CAM_BAD_PERMISSIONS;
|
||
log_debug("[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 == EI2_StationType_roadSideUnit) {
|
||
// Send CAMs if vehicles nearby
|
||
if (bpi->stationType != EI2_StationType_roadSideUnit && bpi->isNeighbour) {
|
||
lightship->t_last_vehicle = now;
|
||
lightship->is_vehicle_near = true;
|
||
}
|
||
} else {
|
||
// Protected zones
|
||
if (cam_c->cam.camParameters.basicContainer.stationType == EI2_StationType_roadSideUnit &&
|
||
cam_c->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
|
||
EI2_ProtectedCommunicationZonesRSU_t *pzs = cam_c->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(EI2_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];
|
||
EIS_ManagementRequest_t* mreq = calloc(1, sizeof(EIS_ManagementRequest_t));
|
||
mreq->present = EIS_ManagementRequest_PR_attributes;
|
||
mreq->choice.attributes.present = EIS_ManagementRequestAttributes_PR_set;
|
||
mreq->choice.attributes.choice.set.protectedZones = calloc(1, sizeof(struct EIS_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_copy(&asn_DEF_EIS_ProtectedCommunicationZone,
|
||
(void**) &mreq->choice.attributes.choice.set.protectedZones->list.array[p],
|
||
lightship->protected_zones.pz[p]
|
||
);
|
||
}
|
||
|
||
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_ManagementRequest, NULL, mreq, b_oer, 512);
|
||
ASN_STRUCT_FREE(asn_DEF_EIS_ManagementRequest, mreq);
|
||
void* management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ);
|
||
itss_0send(management_socket, b_oer, enc.encoded);
|
||
log_debug("[ca]-> sending MReq.attributes.set.protectedZones to ->[management]");
|
||
uint8_t code;
|
||
itss_0recv_rt(&management_socket, &code, 1, b_oer, enc.encoded, 1000);
|
||
itss_0close(management_socket);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
pthread_mutex_unlock(&lightship->lock);
|
||
|
||
return rv;
|
||
}
|
||
|
||
static int check_pz() {
|
||
|
||
lightship_t* lightship = &facilities.lightship;
|
||
|
||
bool is_inside = false;
|
||
|
||
itss_space_lock();
|
||
itss_space_get();
|
||
double lat = epv.space.data.latitude.value/10000000.0;
|
||
double lon = epv.space.data.longitude.value/10000000.0;
|
||
itss_space_unlock();
|
||
|
||
pthread_mutex_lock(&lightship->lock);
|
||
|
||
for (int i = 0; i < lightship->protected_zones.pz_len; ++i) {
|
||
double d = it2s_geodesy_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() {
|
||
int rv = 0;
|
||
|
||
EIS_NetworkingRequest_t* nr = calloc(1, sizeof(EIS_NetworkingRequest_t));
|
||
nr->present = EIS_NetworkingRequest_PR_packet;
|
||
EIS_NetworkingPacketRequest_t* npr = &nr->choice.packet;
|
||
|
||
npr->network.present = EIS_NetworkingPacketRequestNW_PR_gn;
|
||
npr->network.choice.gn.trafficClass = 2;
|
||
npr->network.choice.gn.destinationAddress.buf = malloc(6);
|
||
for (int i = 0; i < 6; ++i) {
|
||
npr->network.choice.gn.destinationAddress.buf[i] = 0xff;
|
||
}
|
||
npr->network.choice.gn.destinationAddress.size = 6;
|
||
npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb;
|
||
npr->network.choice.gn.securityProfile.sign = true;
|
||
|
||
npr->transport.present = EIS_NetworkingPacketRequestTP_PR_btp;
|
||
npr->transport.choice.btp.btpType = EIS_BTPType_btpB;
|
||
npr->transport.choice.btp.destinationPort = EIS_Port_cam;
|
||
if (facilities.edm.enabled) {
|
||
npr->transport.choice.btp.destinationPortInfo = calloc(1, sizeof(OCTET_STRING_t));
|
||
npr->transport.choice.btp.destinationPortInfo->size = 2;
|
||
npr->transport.choice.btp.destinationPortInfo->buf = malloc(2);
|
||
*(uint16_t*)npr->transport.choice.btp.destinationPortInfo->buf = 0xed;
|
||
}
|
||
|
||
const int buf_len = 1024;
|
||
npr->data.buf = malloc(buf_len);
|
||
|
||
// Fill header for FacilitiesIndication and FacilitiesMessageIndication structs
|
||
|
||
EIS_FacilitiesIndication_t* fi = calloc(1,sizeof(EIS_FacilitiesIndication_t));
|
||
fi->present = EIS_FacilitiesIndication_PR_message;
|
||
EIS_FacilitiesMessageIndication_t* fmi = &fi->choice.message;
|
||
fmi->itsMessageType = EIS_ItsMessageType_cam;
|
||
fmi->data.buf = malloc(buf_len);
|
||
|
||
uint8_t nr_oer[buf_len];
|
||
uint8_t fi_oer[buf_len];
|
||
nr_oer[0] = 4; // Facilities
|
||
fi_oer[0] = 4;
|
||
while (!facilities.exit) {
|
||
itss_usleep(50*1000);
|
||
|
||
if (lightship_check() && facilities.lightship.active) {
|
||
rv = mk_cam(npr->data.buf, (uint16_t*) &npr->data.size);
|
||
if (rv) {
|
||
continue;
|
||
}
|
||
|
||
if (facilities.edm.enabled) {
|
||
edm_encap(npr->data.buf, (uint16_t*) &npr->data.size, buf_len, EIS_Port_cam);
|
||
}
|
||
|
||
memcpy(fmi->data.buf, npr->data.buf, npr->data.size);
|
||
fmi->data.size = npr->data.size;
|
||
|
||
// Check if inside PZ
|
||
npr->network.choice.gn.communicationProfile = 0;
|
||
if (facilities.station_type != 15 && check_pz()) npr->network.choice.gn.communicationProfile = 1;
|
||
|
||
uint32_t id = itss_id(npr->data.buf, npr->data.size);
|
||
npr->id = id;
|
||
fmi->id = id;
|
||
|
||
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_NetworkingRequest, NULL, nr, nr_oer+1, 1023);
|
||
if (enc.encoded == -1) {
|
||
log_error("encoding TR for cam failed");
|
||
continue;
|
||
}
|
||
|
||
asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesIndication, NULL, fi, fi_oer+1, 1023);
|
||
if(enc_fdi.encoded == -1){
|
||
log_error("encoding FI for cam failed");
|
||
continue;
|
||
}
|
||
|
||
itss_queue_send(facilities.tx_queue, itss_queue_packet_new(nr_oer, enc.encoded+1, ITSS_NETWORKING, id, "NR.packet.btp"));
|
||
|
||
itss_queue_send(facilities.tx_queue, itss_queue_packet_new(fi_oer, enc_fdi.encoded+1, ITSS_APPLICATIONS, id, "FI.message"));
|
||
|
||
lightship_reset_timer();
|
||
|
||
// Logging
|
||
if (facilities.logging.dbms) {
|
||
pthread_mutex_lock(&facilities.id.lock);
|
||
uint32_t station_id = facilities.id.station_id;
|
||
pthread_mutex_unlock(&facilities.id.lock);
|
||
itss_db_add(facilities.logging.dbms, station_id, npr->id, true, EI1_messageID_cam, NULL, npr->data.buf, npr->data.size);
|
||
}
|
||
|
||
if (facilities.logging.recorder) {
|
||
uint16_t buffer_len = 2048;
|
||
uint8_t buffer[buffer_len];
|
||
int e = itss_management_record_packet_sdu(
|
||
buffer,
|
||
buffer_len,
|
||
npr->data.buf,
|
||
npr->data.size,
|
||
id,
|
||
itss_time_get(),
|
||
ITSS_FACILITIES,
|
||
true);
|
||
if (e != -1) {
|
||
itss_queue_send(facilities.tx_queue, itss_queue_packet_new(buffer, e, ITSS_MANAGEMENT, id, "MReq.packet.set"));
|
||
}
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingRequest, nr);
|
||
ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesIndication, fi);
|
||
|
||
return NULL;
|
||
}
|