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

Merge branch 'master' into 'feature-cpm-v2' 

# Conflicts:
#   src/cam.c
This commit is contained in:
Diogo Jesus 2024-03-21 14:36:24 +00:00
parent af66066235 5478d29a42
commit de2a3a6f8b
17 changed files with 389 additions and 865 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.gitlab-ci.yml
View File

@ -8,7 +8,7 @@ stages:
#.dependencies:
# before_script:
# - pacman -Sy zeromq it2s-config-git it2s-asn-git it2s-tender-git mariadb it2s-gps-git it2s-obd-git --overwrite=* --noconfirm
# - pacman -Sy zeromq it2s-config-git it2s-asn-git it2s-tender-git mariadb it2s-gnss-git it2s-obd-git --overwrite=* --noconfirm
#build:
# stage: build
@ -29,14 +29,16 @@ stages:
deploy release:
stage: deploy release
resource_group: packaging
script:
- curl -fs http://192.168.94.221:3000/archlinux/x86/it2s-itss-facilities-git
- curl -fs http://192.168.94.221:3000/ubuntu/focal/arm64/it2s-itss-facilities-git
- curl -fs http://192.168.94.221:3000/ubuntu/mantic/amd64/it2s-itss-facilities-git
- curl --fail-with-body --silent http://192.168.94.221:3000/archlinux/x86/it2s-itss-facilities-git
- curl --fail-with-body --silent http://192.168.94.221:3000/ubuntu/focal/arm64/it2s-itss-facilities-git
- curl --fail-with-body --silent http://192.168.94.221:3000/ubuntu/mantic/amd64/it2s-itss-facilities-git
deploy debug:
stage: deploy debug
resource_group: packaging
script:
- curl -fs http://192.168.94.221:3000/archlinux/x86/it2s-itss-facilities-debug-git
- curl -fs http://192.168.94.221:3000/ubuntu/focal/arm64/it2s-itss-facilities-debug-git
- curl -fs http://192.168.94.221:3000/ubuntu/mantic/amd64/it2s-itss-facilities-debug-git
- curl --fail-with-body --silent http://192.168.94.221:3000/archlinux/x86/it2s-itss-facilities-debug-git
- curl --fail-with-body --silent http://192.168.94.221:3000/ubuntu/focal/arm64/it2s-itss-facilities-debug-git
- curl --fail-with-body --silent http://192.168.94.221:3000/ubuntu/mantic/amd64/it2s-itss-facilities-debug-git

2
src/CMakeLists.txt
View File

@ -21,6 +21,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
-lit2s-asn-etsi-its-sdu-itss-networking
-lit2s-asn-etsi-its-sdu-cdd-1.3.1
-lzmq
-lit2s_gnss
-lpthread
-lit2s-config-etsi-its
-lit2s-asn-etsi-its-v1-cdd-1.3.1
@ -35,6 +36,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
-lit2s-asn-etsi-its-v1-evrsrm
-lit2s-asn-etsi-its-v2-cdd-2.2.1
-lit2s-asn-etsi-its-v2-cam
-lit2s-asn-etsi-its-v2-denm
-lit2s-tender
-lm
-lrt

543
src/cam.c
View File

@ -97,7 +97,7 @@ static void path_history_update(pos_point_t* pn) {
if (ls->concise_points_len == 3) {
double actual_error;
double actual_chord_length = itss_haversine(
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,
@ -140,7 +140,7 @@ static void path_history_update(pos_point_t* pn) {
if (ls->path_history_len >= 2) {
double distance = 0.0;
for (int i = 0; i < ls->path_history_len - 1; ++i) {
distance += itss_haversine(
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,
@ -157,251 +157,7 @@ static void path_history_update(pos_point_t* pn) {
}
}
static int mk_cam_v1(uint8_t *cam_oer, uint16_t *cam_len) {
int rv = 0;
int shm_fd, shm_valid = 0;
lightship_t* lightship = &facilities.lightship;
EI1_CAM_t *cam = calloc(1, sizeof(EI1_CAM_t));
cam->header.protocolVersion = 2;
cam->header.messageID = EI1_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;
EI1_BasicContainer_t* bc = &cam->cam.camParameters.basicContainer;
uint64_t now = itss_time_get();
pthread_mutex_lock(&lightship->lock);
if (facilities.station_type != EI1_StationType_roadSideUnit) {
cam->cam.generationDeltaTime = now % 65536;
itss_space_lock();
itss_space_get();
bc->referencePosition.altitude.altitudeValue = epv.space.altitude;
bc->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
// Set GPS coordinates
bc->referencePosition.latitude = epv.space.latitude;
bc->referencePosition.longitude = epv.space.longitude;
uint16_t lat_conf = epv.space.latitude_conf;
uint16_t lon_conf = epv.space.longitude_conf;
cam->cam.camParameters.highFrequencyContainer.present = EI1_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
EI1_BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
// Set speed
bvc_hf->speed.speedValue = epv.space.speed;
bvc_hf->speed.speedConfidence = epv.space.speed_conf;
// Set heading
bvc_hf->heading.headingValue = epv.space.heading;
bvc_hf->heading.headingConfidence = epv.space.heading_conf;
itss_space_unlock(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 = EI1_VehicleLengthConfidenceIndication_unavailable;
bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = EI1_LongitudinalAccelerationValue_unavailable;
bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = EI1_AccelerationConfidence_unavailable;
bvc_hf->driveDirection = EI1_DriveDirection_unavailable;
bvc_hf->curvature.curvatureValue = EI1_CurvatureValue_unavailable;
bvc_hf->curvature.curvatureConfidence = EI1_CurvatureConfidence_unavailable;
bvc_hf->yawRate.yawRateValue = EI1_YawRateValue_unavailable;
bvc_hf->yawRate.yawRateConfidence = EI1_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.longitudinalAccelerationValue = long_a;
bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = EI1_AccelerationConfidence_unavailable;
} else {
bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = EI1_LongitudinalAccelerationValue_unavailable;
bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = EI1_AccelerationConfidence_unavailable;
}
// Low frequency container
if (now > lightship->t_last_cam_lfc + 500) {
cam->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(EI1_LowFrequencyContainer_t));
cam->cam.camParameters.lowFrequencyContainer->present = EI1_LowFrequencyContainer_PR_basicVehicleContainerLowFrequency;
EI1_BasicVehicleContainerLowFrequency_t* bvc_lf = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency;
EI1_PathHistory_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(EI1_PathPoint_t));
if (lightship->path_history[0]->alt != EI1_AltitudeValue_unavailable && pn.alt != EI1_AltitudeValue_unavailable) {
ph->list.array[0]->pathPosition.deltaAltitude = lightship->path_history[0]->alt - pn.alt;
} else {
ph->list.array[0]->pathPosition.deltaAltitude = EI1_DeltaAltitude_unavailable;
}
if (lightship->path_history[0]->lat != EI1_Latitude_unavailable && pn.lat != EI1_Latitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLatitude = lightship->path_history[0]->lat - pn.lat;
} else {
ph->list.array[0]->pathPosition.deltaLatitude = EI1_DeltaLatitude_unavailable;
}
if (lightship->path_history[0]->lon != EI1_Longitude_unavailable && pn.lon != EI1_Longitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLongitude = lightship->path_history[0]->lon - pn.lon;
} else {
ph->list.array[0]->pathPosition.deltaLongitude = EI1_DeltaLongitude_unavailable;
}
ph->list.array[0]->pathDeltaTime = calloc(1,sizeof(EI1_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(EI1_PathPoint_t));
if (lightship->path_history[i]->alt != EI1_AltitudeValue_unavailable && lightship->path_history[i-1]->alt != EI1_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 = EI1_DeltaAltitude_unavailable;
}
if (lightship->path_history[i]->lat != EI1_Latitude_unavailable && lightship->path_history[i-1]->lat != EI1_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 = EI1_DeltaLatitude_unavailable;
}
if (lightship->path_history[i]->lon != EI1_Longitude_unavailable && lightship->path_history[i-1]->lon != EI1_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 = EI1_DeltaLongitude_unavailable;
}
ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(EI1_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.altitude;
bc->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
// Set GPS coordinates
bc->referencePosition.latitude = epv.space.latitude;
bc->referencePosition.longitude = epv.space.longitude;
bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = EI1_SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = EI1_SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = EI1_HeadingValue_unavailable;
itss_space_unlock();
cam->cam.camParameters.highFrequencyContainer.present = EI1_HighFrequencyContainer_PR_rsuContainerHighFrequency;
if (lightship->protected_zones.pz_len > 0) {
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(EI1_ProtectedCommunicationZonesRSU_t));
EI1_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(EI1_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_EI1_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_EI1_CAM, cam);
return rv;
}
static int mk_cam_v2(uint8_t *cam_oer, uint16_t *cam_len) {
static int mk_cam(uint8_t *cam_oer, uint16_t *cam_len) {
int rv = 0;
int shm_fd, shm_valid = 0;
@ -427,25 +183,25 @@ static int mk_cam_v2(uint8_t *cam_oer, uint16_t *cam_len) {
itss_space_lock();
itss_space_get();
bc->referencePosition.altitude.altitudeValue = epv.space.altitude;
bc->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
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.latitude;
bc->referencePosition.longitude = epv.space.longitude;
uint16_t lat_conf = epv.space.latitude_conf;
uint16_t lon_conf = epv.space.longitude_conf;
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.speed;
bvc_hf->speed.speedConfidence = epv.space.speed_conf;
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.heading;
bvc_hf->heading.headingConfidence = epv.space.heading_conf;
bvc_hf->heading.headingValue = epv.space.data.heading.value;
bvc_hf->heading.headingConfidence = epv.space.data.heading.confidence;
itss_space_unlock(epv);
@ -587,12 +343,12 @@ static int mk_cam_v2(uint8_t *cam_oer, uint16_t *cam_len) {
itss_space_lock();
itss_space_get();
bc->referencePosition.altitude.altitudeValue = epv.space.altitude;
bc->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
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.latitude;
bc->referencePosition.longitude = epv.space.longitude;
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;
@ -680,19 +436,19 @@ int lightship_check() {
itss_space_get();
// Check heading delta > 4º
int diff = epv.space.heading - lightship->last_pos.heading;
int diff = epv.space.data.heading.value - lightship->last_pos.heading;
diff += (diff > 180) ? -360 : (diff < -180) ? 360 : 0;
if (abs(diff) > 40) rv = 1;
if (!rv) {
// Check speed delta > 0.5 m/s
diff = epv.space.speed - lightship->last_pos.speed;
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.speed + lightship->last_pos.speed)/2 / 100; /* cm/s to m/s */
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;
}
@ -744,235 +500,27 @@ void lightship_reset_timer() {
pthread_mutex_unlock(&lightship->lock);
}
enum CAM_CHECK_R check_cam_v1(EIS_BTPPacketIndication_t *bpi, EI1_CAM_t* cam, uint8_t* ssp, uint32_t ssp_len) {
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->cam.camParameters.highFrequencyContainer.present == EI1_HighFrequencyContainer_PR_rsuContainerHighFrequency &&
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
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->cam.camParameters.specialVehicleContainer) {
switch (cam->cam.camParameters.specialVehicleContainer->present) {
case EI1_SpecialVehicleContainer_PR_NOTHING:
break;
case EI1_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 EI1_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 EI1_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 EI1_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->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 EI1_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 EI1_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->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;
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 EI1_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->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) {
switch (*cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) {
case EI1_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 EI1_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->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 == EI1_StationType_roadSideUnit) {
// Send CAMs if vehicles nearby
if (bpi->stationType != EI1_StationType_roadSideUnit && bpi->isNeighbour) {
lightship->t_last_vehicle = now;
lightship->is_vehicle_near = true;
}
} else {
// Protected zones
if (cam->cam.camParameters.basicContainer.stationType == EI1_StationType_roadSideUnit &&
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
EI1_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(EI1_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;
}
enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t *bpi, EI2_CAM_t* cam, uint8_t* ssp, uint32_t ssp_len) {
int rv = 0;
lightship_t* lightship = &facilities.lightship;
uint64_t now = itss_time_get();
// Check permissions
if (ssp) {
if (cam->cam.camParameters.highFrequencyContainer.present == EI2_HighFrequencyContainer_PR_rsuContainerHighFrequency &&
cam->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->cam.camParameters.specialVehicleContainer) {
switch (cam->cam.camParameters.specialVehicleContainer->present) {
if (cam_c->cam.camParameters.specialVehicleContainer) {
switch (cam_c->cam.camParameters.specialVehicleContainer->present) {
case EI2_SpecialVehicleContainer_PR_NOTHING:
break;
case EI2_SpecialVehicleContainer_PR_publicTransportContainer:
@ -1002,7 +550,7 @@ enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t *bpi, EI2_CAM_t* cam, ui
log_debug("[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 (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);
@ -1023,10 +571,10 @@ enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t *bpi, EI2_CAM_t* cam, ui
log_debug("[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) {
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->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf;
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;
@ -1049,8 +597,8 @@ enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t *bpi, EI2_CAM_t* cam, ui
log_debug("[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) {
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;
@ -1070,7 +618,7 @@ enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t *bpi, EI2_CAM_t* cam, ui
}
}
if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.speedLimit) {
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);
@ -1091,9 +639,9 @@ enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t *bpi, EI2_CAM_t* cam, ui
}
} else {
// Protected zones
if (cam->cam.camParameters.basicContainer.stationType == EI2_StationType_roadSideUnit &&
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
EI2_ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
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;
@ -1174,14 +722,14 @@ static int check_pz() {
itss_space_lock();
itss_space_get();
double lat = epv.space.latitude/10000000.0;
double lon = epv.space.longitude/10000000.0;
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 = itss_haversine(lat, lon, (double) lightship->protected_zones.pz[i]->protectedZoneLatitude/10000000.0, (double) lightship->protected_zones.pz[i]->protectedZoneLongitude/10000000.0);
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) {
@ -1240,15 +788,10 @@ void* ca_service() {
nr_oer[0] = 4; // Facilities
fi_oer[0] = 4;
while (!facilities.exit) {
usleep(1000*50);
itss_usleep(50*1000);
if (lightship_check() && facilities.lightship.active) {
switch (facilities.mver.defaultv) {
case MVER_1:
case MVER_2:
rv = mk_cam_v2(npr->data.buf, (uint16_t*) &npr->data.size);
break;
}
rv = mk_cam(npr->data.buf, (uint16_t*) &npr->data.size);
if (rv) {
continue;
}

3
src/cam.h
View File

@ -117,8 +117,7 @@ void lightship_reset_timer();
*
* @return A CAM check code
*/
enum CAM_CHECK_R check_cam_v1(EIS_BTPPacketIndication_t* bpi, EI1_CAM_t* cam, uint8_t* ssp, uint32_t ssp_len);
enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t* bpi, EI2_CAM_t* cam, uint8_t* ssp, uint32_t ssp_len);
enum CAM_CHECK_R check_cam(EIS_BTPPacketIndication_t* bpi, void* cam, uint8_t* ssp, uint32_t ssp_len);
/*
* @brief Main CA service

56
src/config.c
View File

@ -317,14 +317,14 @@ int facilities_config() {
// Message version
if (!strcmp("v1", etsi_its_cfg->facilities.mver.default_version)) {
facilities.mver.defaultv = MVER_1;
log_error("[config] only default message version 2 is supported");
return -1;
} else if (!strcmp("v2", etsi_its_cfg->facilities.mver.default_version)) {
facilities.mver.defaultv = MVER_2;
facilities.pver.defaultv = 2;
} else {
log_warn("[config] unrecognized default messages version :: using version 1");
facilities.mver.defaultv = MVER_1;
log_warn("[config] unrecognized default messages version :: using version 2");
facilities.pver.defaultv = 2;
}
facilities.mver.handle_all = etsi_its_cfg->facilities.mver.handle_all;
// DENM
facilities.den.n_max_events = etsi_its_cfg->facilities.denm.nmax_active_events;
@ -495,23 +495,14 @@ int facilities_config() {
}
}
itss_epv_init();
facilities.edm.enabled = itss_cfg->applications.extensions.enabled;
EIS_ManagementRequest_t* mreq = calloc(1, sizeof(EIS_ManagementRequest_t));
mreq->present = EIS_ManagementRequest_PR_attributes;
mreq->choice.attributes.present = EIS_ManagementRequestAttributes_PR_get;
mreq->choice.attributes.choice.get.coordinates = 1;
mreq->choice.attributes.choice.get.altitude = 1;
mreq->choice.attributes.choice.get.heading = 1;
mreq->choice.attributes.choice.get.speed = 1;
mreq->choice.attributes.choice.get.acceleration = 1;
mreq->choice.attributes.choice.get.gpsType = 1;
mreq->choice.attributes.choice.get.clockType = 1;
mreq->choice.attributes.choice.get.clock = 1;
mreq->choice.attributes.choice.get.clockOffset = 1;
mreq->choice.attributes.choice.get.trajectory = etsi_its_cfg->facilities.mcm.activate;
void* management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ);
uint8_t b_tx[256], b_rx[256];
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_ManagementRequest, NULL, mreq, b_tx, 256);
@ -529,42 +520,12 @@ int facilities_config() {
long lat, lon, alt, alt_conf;
if (mrep->returnCode == EIS_ManagementReplyReturnCode_accepted &&
mrep->data &&
mrep->data->choice.attributes.coordinates &&
mrep->data->choice.attributes.altitude &&
mrep->data->choice.attributes.heading &&
mrep->data->choice.attributes.speed &&
mrep->data->choice.attributes.acceleration &&
mrep->data->choice.attributes.clockType &&
mrep->data->choice.attributes.clock &&
mrep->data->choice.attributes.clockOffset &&
mrep->data->choice.attributes.gpsType &&
(!!mrep->data->choice.attributes.trajectory == mreq->choice.attributes.choice.get.trajectory)) {
epv.space.latitude = mrep->data->choice.attributes.coordinates->latitude;
epv.space.latitude_conf = mrep->data->choice.attributes.coordinates->latitudeConfidence;
epv.space.longitude = mrep->data->choice.attributes.coordinates->longitude;
epv.space.longitude_conf = mrep->data->choice.attributes.coordinates->longitudeConfidence;
epv.space.altitude = mrep->data->choice.attributes.altitude->altitudeValue;
epv.space.altitude_conf = mrep->data->choice.attributes.altitude->altitudeConfidence;
epv.space.heading = mrep->data->choice.attributes.heading->headingValue;
epv.space.heading_conf = mrep->data->choice.attributes.heading->headingConfidence;
epv.space.speed = mrep->data->choice.attributes.speed->speedValue;
epv.space.speed_conf = mrep->data->choice.attributes.speed->speedConfidence;
epv.space.accel = mrep->data->choice.attributes.acceleration->longitudinalAccelerationValue;
mrep->data->choice.attributes.clockOffset) {
epv.space.type = *mrep->data->choice.attributes.gpsType;
epv.time.type = *mrep->data->choice.attributes.clockType;
itss_epv_init(*mrep->data->choice.attributes.clockType, TIME_MILLISECONDS, itss_cfg->time.simulated.source);
asn_INTEGER2ulong(mrep->data->choice.attributes.clock, &epv.time.clock);
asn_INTEGER2ulong(mrep->data->choice.attributes.clockOffset, &epv.time.offset);
if (etsi_its_cfg->facilities.mcm.activate) {
epv.trajectory.len = mrep->data->choice.attributes.trajectory->list.count;
for (int i = 0; i < mrep->data->choice.attributes.trajectory->list.count; ++i) {
epv.trajectory.path[i].latitude = mrep->data->choice.attributes.trajectory->list.array[i]->latitude;
epv.trajectory.path[i].longitude = mrep->data->choice.attributes.trajectory->list.array[i]->longitude;
asn_INTEGER2ulong(&mrep->data->choice.attributes.trajectory->list.array[i]->timestamp, &epv.trajectory.path[i].timestamp);
}
}
asn_INTEGER2uint64(mrep->data->choice.attributes.clockOffset, &epv.time.offset);
} else {
log_error("rejected MR attribute request");
rv = 1;
@ -573,6 +534,7 @@ int facilities_config() {
ASN_STRUCT_FREE(asn_DEF_EIS_ManagementRequest, mreq);
ASN_STRUCT_FREE(asn_DEF_EIS_ManagementReply, mrep);
if (etsi_its_cfg->facilities.saem.activate) { // TODO handle various services
facilities.bulletin.to_provide_len = 1;
facilities.bulletin.to_provide[0] = calloc(1, sizeof(announcement_t));

10
src/cpm.c
View File

@ -572,10 +572,10 @@ static int mk_cpm(uint8_t *bdr_oer, uint32_t *bdr_len, uint8_t *fdi_oer, uint32_
int32_t lat, lon, alt, alt_conf;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
alt = epv.space.altitude;
alt_conf = epv.space.altitude_conf;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
alt = epv.space.data.altitude.value;
alt_conf = epv.space.data.altitude.confidence;
itss_space_unlock();
cpm_tx->cpm.generationDeltaTime = generationDeltaTime;
@ -744,7 +744,7 @@ void *cp_service(){
is_radar_connected = radar_connection(RADAR_PORT);
while(!facilities.exit){
usleep(1000*50);
itss_usleep(1000*50);
/* If the Radar is not connected to TMC, a TCP socket is needed to fool the Radar */
/* To maintain the connection the content must be read */

161
src/denm.c
View File

@ -6,6 +6,8 @@
#include <it2s-tender/time.h>
#include <it2s-tender/space.h>
#define DENM_VERSION(denm) (((EI1_ItsPduHeader_t*)(denm))->protocolVersion)
const cc_ssp_bm_t CC_SSP_BM_MAP[] = {
{"trafficCondition", 1, 0x80000000},
{"accident", 2, 0x40000000},
@ -67,29 +69,56 @@ static int permissions_check(int cause_code, uint8_t* permissions, uint8_t permi
}
static enum EVENT_CHECK_R event_check(EI1_DENM_t *denm, uint8_t* ssp, uint32_t ssp_len) {
static enum EVENT_CHECK_R event_check(void *denm, uint8_t* ssp, uint32_t ssp_len) {
int rv = 0;
den_t* den = &facilities.den;
uint8_t cause_code;
uint32_t action_id_origin_station;
uint32_t action_id_seq_num;
switch (DENM_VERSION(denm)) {
case 1:
;
EI1_DENM_t* denm_v1 = (EI1_DENM_t*) denm;
cause_code = denm_v1->denm.situation->eventType.causeCode;
action_id_origin_station = denm_v1->denm.management.actionID.originatingStationID;
action_id_seq_num = denm_v1->denm.management.actionID.sequenceNumber;
break;
case 2:
;
EI2_DENM_t* denm_v2 = (EI2_DENM_t*) denm;
cause_code = denm_v2->denm.situation->eventType.ccAndScc.present - 1;
action_id_origin_station = denm_v2->denm.management.actionId.originatingStationId;
action_id_seq_num = denm_v2->denm.management.actionId.sequenceNumber;
break;
default:
return EVENT_INVALID;
}
/* Common fields in version 1, 2 */
EI2_DENM_t* denm_c = (EI2_DENM_t*) denm;
uint64_t e_detection_time, e_reference_time;
asn_INTEGER2ulong((INTEGER_t*) &denm->denm.management.detectionTime, &e_detection_time);
asn_INTEGER2ulong((INTEGER_t*) &denm->denm.management.referenceTime, &e_reference_time);
asn_INTEGER2ulong((INTEGER_t*) &denm_c->denm.management.detectionTime, &e_detection_time);
asn_INTEGER2ulong((INTEGER_t*) &denm_c->denm.management.referenceTime, &e_reference_time);
if (e_detection_time > e_reference_time) {
return EVENT_INVALID;
}
// Check if event cause code type is permitted by the issuing ticket
if (ssp && denm->denm.situation) {
if (!permissions_check(denm->denm.situation->eventType.causeCode, ssp, ssp_len)) {
if (ssp && denm_c->denm.situation) {
if (!permissions_check(cause_code, ssp, ssp_len)) {
return EVENT_BAD_PERMISSIONS;
}
}
uint32_t e_validity_duration;
if (denm->denm.management.validityDuration != NULL) {
e_validity_duration = *(uint32_t *) denm->denm.management.validityDuration * 1000; // validityDuration comes in seconds
if (denm_c->denm.management.validityDuration != NULL) {
e_validity_duration = *(uint32_t *) denm_c->denm.management.validityDuration * 1000; // validityDuration comes in seconds
} else {
e_validity_duration = 600 * 1000;
}
@ -108,8 +137,8 @@ static enum EVENT_CHECK_R event_check(EI1_DENM_t *denm, uint8_t* ssp, uint32_t s
for (int i = 0; i < den->n_max_events; ++i) {
if (den->events[i]->enabled) {
if (den->events[i]->station_id == denm->denm.management.actionID.originatingStationID &&
den->events[i]->sn == denm->denm.management.actionID.sequenceNumber) {
if (den->events[i]->station_id == action_id_origin_station &&
den->events[i]->sn == action_id_seq_num) {
is_new = false;
@ -133,10 +162,10 @@ static enum EVENT_CHECK_R event_check(EI1_DENM_t *denm, uint8_t* ssp, uint32_t s
if (is_update) {
if (denm->denm.management.termination != NULL) {
if (*denm->denm.management.termination == 0) {
if (denm_c->denm.management.termination != NULL) {
if (*denm_c->denm.management.termination == 0) {
return EVENT_CANCELLATION;
} else if (*denm->denm.management.termination == 1) {
} else if (*denm_c->denm.management.termination == 1) {
return EVENT_NEGATION;
}
}
@ -155,18 +184,44 @@ static enum EVENT_CHECK_R event_check(EI1_DENM_t *denm, uint8_t* ssp, uint32_t s
return EVENT_NEW;
}
static int event_add(EI1_DENM_t *denm, uint64_t* id) {
static int event_add(void *denm, uint64_t* id) {
den_t* den = &facilities.den;
uint64_t now = itss_time_get();
uint8_t cause_code;
uint32_t action_id_origin_station;
uint32_t action_id_seq_num;
switch (DENM_VERSION(denm)) {
case 1:
;
EI1_DENM_t* denm_v1 = (EI1_DENM_t*) denm;
cause_code = denm_v1->denm.situation->eventType.causeCode;
action_id_origin_station = denm_v1->denm.management.actionID.originatingStationID;
action_id_seq_num = denm_v1->denm.management.actionID.sequenceNumber;
break;
case 2:
;
EI2_DENM_t* denm_v2 = (EI2_DENM_t*) denm;
cause_code = denm_v2->denm.situation->eventType.ccAndScc.present - 1;
action_id_origin_station = denm_v2->denm.management.actionId.originatingStationId;
action_id_seq_num = denm_v2->denm.management.actionId.sequenceNumber;
break;
default:
return EVENT_INVALID;
}
/* Common fields in version 1, 2 */
EI2_DENM_t* denm_c = (EI2_DENM_t*) denm;
uint64_t e_detection_time, e_reference_time;
asn_INTEGER2ulong((INTEGER_t*) &denm->denm.management.detectionTime, &e_detection_time);
asn_INTEGER2ulong((INTEGER_t*) &denm->denm.management.referenceTime, &e_reference_time);
asn_INTEGER2ulong((INTEGER_t*) &denm_c->denm.management.detectionTime, &e_detection_time);
asn_INTEGER2ulong((INTEGER_t*) &denm_c->denm.management.referenceTime, &e_reference_time);
uint32_t e_validity_duration;
if (denm->denm.management.validityDuration != NULL) {
e_validity_duration = *(uint32_t *) denm->denm.management.validityDuration * 1000; // validityDuration comes in seconds
if (denm_c->denm.management.validityDuration != NULL) {
e_validity_duration = *(uint32_t *) denm_c->denm.management.validityDuration * 1000; // validityDuration comes in seconds
} else {
e_validity_duration = 600 * 1000;
}
@ -182,10 +237,10 @@ static int event_add(EI1_DENM_t *denm, uint64_t* id) {
}
uint8_t state = EVENT_ACTIVE;
if (denm->denm.management.termination != NULL) {
if (*(denm->denm.management.termination) == 0) {
if (denm_c->denm.management.termination != NULL) {
if (*(denm_c->denm.management.termination) == 0) {
state = EVENT_CANCELLED;
} else if (*(denm->denm.management.termination) == 1) {
} else if (*(denm_c->denm.management.termination) == 1) {
state = EVENT_NEGATED;
}
}
@ -206,17 +261,17 @@ static int event_add(EI1_DENM_t *denm, uint64_t* id) {
den->events[index]->id = *id;
den->events[index]->enabled = true;
den->events[index]->state = state;
den->events[index]->station_id = denm->denm.management.actionID.originatingStationID;
den->events[index]->sn = denm->denm.management.actionID.sequenceNumber;
den->events[index]->station_id = action_id_origin_station;
den->events[index]->sn = action_id_seq_num;
den->events[index]->detection_time = e_detection_time;
den->events[index]->reference_time = e_reference_time;
den->events[index]->expiration_time = e_detection_time + e_validity_duration;
den->events[index]->latitude = denm->denm.management.eventPosition.latitude;
den->events[index]->longitude = denm->denm.management.eventPosition.longitude;
den->events[index]->latitude = denm_c->denm.management.eventPosition.latitude;
den->events[index]->longitude = denm_c->denm.management.eventPosition.longitude;
den->events[index]->denm = denm;
if (denm->denm.management.actionID.sequenceNumber > den->sn) {
den->sn = denm->denm.management.actionID.sequenceNumber;
if (action_id_seq_num > den->sn) {
den->sn = action_id_seq_num;
}
}
@ -226,20 +281,46 @@ static int event_add(EI1_DENM_t *denm, uint64_t* id) {
else return 0; // Event added to db
}
static int event_update(EI1_DENM_t *denm, uint64_t* id) {
static int event_update(void *denm, uint64_t* id) {
den_t* den = &facilities.den;
uint64_t now = itss_time_get();
uint8_t cause_code;
uint32_t action_id_origin_station;
uint32_t action_id_seq_num;
switch (DENM_VERSION(denm)) {
case 1:
;
EI1_DENM_t* denm_v1 = (EI1_DENM_t*) denm;
cause_code = denm_v1->denm.situation->eventType.causeCode;
action_id_origin_station = denm_v1->denm.management.actionID.originatingStationID;
action_id_seq_num = denm_v1->denm.management.actionID.sequenceNumber;
break;
case 2:
;
EI2_DENM_t* denm_v2 = (EI2_DENM_t*) denm;
cause_code = denm_v2->denm.situation->eventType.ccAndScc.present - 1;
action_id_origin_station = denm_v2->denm.management.actionId.originatingStationId;
action_id_seq_num = denm_v2->denm.management.actionId.sequenceNumber;
break;
default:
return EVENT_INVALID;
}
/* Common fields in version 1, 2 */
EI2_DENM_t* denm_c = (EI2_DENM_t*) denm;
uint64_t e_detection_time, e_reference_time;
asn_INTEGER2ulong((INTEGER_t*) &denm->denm.management.detectionTime, &e_detection_time);
asn_INTEGER2ulong((INTEGER_t*) &denm->denm.management.referenceTime, &e_reference_time);
asn_INTEGER2ulong((INTEGER_t*) &denm_c->denm.management.detectionTime, &e_detection_time);
asn_INTEGER2ulong((INTEGER_t*) &denm_c->denm.management.referenceTime, &e_reference_time);
uint8_t state = EVENT_ACTIVE;
if (denm->denm.management.termination != NULL) {
if (*(denm->denm.management.termination) == 0) {
if (denm_c->denm.management.termination != NULL) {
if (*(denm_c->denm.management.termination) == 0) {
state = EVENT_CANCELLED;
} else if (*(denm->denm.management.termination) == 1) {
} else if (*(denm_c->denm.management.termination) == 1) {
state = EVENT_NEGATED;
}
}
@ -249,8 +330,8 @@ static int event_update(EI1_DENM_t *denm, uint64_t* id) {
for (int i = 0; i < den->n_max_events; ++i) {
if (den->events[i]->enabled) {
if (den->events[i]->station_id == denm->denm.management.actionID.originatingStationID &&
den->events[i]->sn == denm->denm.management.actionID.sequenceNumber) {
if (den->events[i]->station_id == action_id_origin_station &&
den->events[i]->sn == action_id_seq_num) {
index = i;
break;
@ -289,16 +370,16 @@ static int event_update(EI1_DENM_t *denm, uint64_t* id) {
den->events[index]->detection_time = e_detection_time;
den->events[index]->reference_time = e_reference_time;
if (denm->denm.management.validityDuration != NULL) {
den->events[index]->expiration_time = e_detection_time + *(uint32_t *) denm->denm.management.validityDuration * 1000;
if (denm_c->denm.management.validityDuration != NULL) {
den->events[index]->expiration_time = e_detection_time + *(uint32_t *) denm_c->denm.management.validityDuration * 1000;
}
if (state == EVENT_CANCELLED || state == EVENT_NEGATED) { // Keep terminated events for 10 mins
den->events[index]->expiration_time = now + 600 * 1000;
}
den->events[index]->latitude = denm->denm.management.eventPosition.latitude;
den->events[index]->longitude = denm->denm.management.eventPosition.longitude;
den->events[index]->latitude = denm_c->denm.management.eventPosition.latitude;
den->events[index]->longitude = denm_c->denm.management.eventPosition.longitude;
ASN_STRUCT_FREE(asn_DEF_EI1_DENM, den->events[index]->denm);
den->events[index]->denm = denm;
@ -310,7 +391,7 @@ static int event_update(EI1_DENM_t *denm, uint64_t* id) {
else return 0; // Event updated
}
enum EVENT_CHECK_R event_manage(EI1_DENM_t *denm, uint64_t* id, uint8_t* ssp, uint32_t ssp_len) {
enum EVENT_CHECK_R event_manage(void *denm, uint64_t* id, uint8_t* ssp, uint32_t ssp_len) {
int rv = 0;
switch (rv = event_check(denm, ssp, ssp_len)) {
case EVENT_NEW:
@ -432,7 +513,7 @@ void* den_service() {
pthread_mutex_unlock(&den->lock);
++sleep_count;
usleep(sleep4us);
itss_usleep(sleep4us);
}
return NULL;

5
src/denm.h
View File

@ -3,6 +3,7 @@
#include <stdint.h>
#include <it2s-asn/etsi-its-v1/denm/EI1_DENM.h>
#include <it2s-asn/etsi-its-v2/denm/EI2_DENM.h>
#include <pthread.h>
#include <stdbool.h>
@ -16,7 +17,7 @@ enum EVENT_STATE {
typedef struct event {
uint64_t id;
EI1_DENM_t *denm;
void *denm;
uint32_t station_id;
uint32_t sn;
uint64_t detection_time;
@ -67,7 +68,7 @@ typedef struct cc_ssp_bm {
* @param ssp permissions
* @return 0 if event OK, 1 if event NOK
*/
enum EVENT_CHECK_R event_manage(EI1_DENM_t* denm, uint64_t* id, uint8_t* ssp, uint32_t ssp_len);
enum EVENT_CHECK_R event_manage(void* denm, uint64_t* id, uint8_t* ssp, uint32_t ssp_len);
int den_init();
void* den_service();

6
src/evm.c
View File

@ -61,8 +61,8 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) {
itss_space_get();
cs0->chargingStationLocation.latitude = 405970830;
cs0->chargingStationLocation.longitude = -86628610;
cs0->chargingStationLocation.altitude.altitudeValue = epv.space.altitude;
cs0->chargingStationLocation.altitude.altitudeConfidence = epv.space.altitude_conf;
cs0->chargingStationLocation.altitude.altitudeValue = epv.space.data.altitude.value;
cs0->chargingStationLocation.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
cs0->chargingStationLocation.positionConfidenceEllipse.semiMajorConfidence = EI1_SemiAxisLength_unavailable;
cs0->chargingStationLocation.positionConfidenceEllipse.semiMinorConfidence = EI1_SemiAxisLength_unavailable;
cs0->chargingStationLocation.positionConfidenceEllipse.semiMajorOrientation = EI1_HeadingValue_unavailable;
@ -410,7 +410,7 @@ void *evcsn_service() {
tr_oer[0] = 4; // Facilities
fi_oer[0] = 4;
while (!facilities.exit) {
usleep(1000 * 1000);
itss_usleep(1000 * 1000);
if (facilities.evm_args.activate) {
rv = mk_evcsnm(npr->data.buf, (uint32_t *)&npr->data.size);
if (rv) {

26
src/facilities.c
View File

@ -21,7 +21,6 @@
#include <it2s-tender/recorder.h>
#include <it2s-tender/space.h>
#include <it2s-tender/time.h>
#include <it2s-tender/trajectory.h>
#include <signal.h>
#include <stdbool.h>
#include <sys/stat.h>
@ -274,31 +273,6 @@ static int management_indication(void *responder, uint8_t *msg, uint32_t msg_len
itss_0send(responder, &code, 1);
if (mi->present == EIS_ManagementIndication_PR_attributes) {
itss_space_lock();
epv.space.latitude = mi->choice.attributes.coordinates.latitude;
epv.space.latitude_conf = mi->choice.attributes.coordinates.latitudeConfidence;
epv.space.longitude = mi->choice.attributes.coordinates.longitude;
epv.space.longitude_conf = mi->choice.attributes.coordinates.longitudeConfidence;
epv.space.speed = mi->choice.attributes.speed.speedValue;
epv.space.speed_conf = mi->choice.attributes.speed.speedConfidence;
epv.space.heading = mi->choice.attributes.heading.headingValue;
epv.space.heading_conf = mi->choice.attributes.heading.headingConfidence;
epv.space.accel = mi->choice.attributes.acceleration.longitudinalAccelerationValue;
epv.space.altitude = mi->choice.attributes.altitude.altitudeValue;
epv.space.altitude_conf = mi->choice.attributes.altitude.altitudeConfidence;
itss_space_unlock();
itss_trajectory_lock();
if (mi->choice.attributes.trajectory) {
epv.trajectory.len = mi->choice.attributes.trajectory->list.count;
for (int i = 0; i < mi->choice.attributes.trajectory->list.count; ++i) {
epv.trajectory.path[i].latitude = mi->choice.attributes.trajectory->list.array[i]->latitude;
epv.trajectory.path[i].longitude = mi->choice.attributes.trajectory->list.array[i]->longitude;
asn_INTEGER2ulong(&mi->choice.attributes.trajectory->list.array[i]->timestamp, &epv.trajectory.path[i].timestamp);
}
}
itss_trajectory_unlock();
itss_time_lock();
asn_INTEGER2ulong(&mi->choice.attributes.clock, &epv.time.clock);
itss_time_unlock();

9
src/facilities.h
View File

@ -21,11 +21,6 @@
#include <it2s-tender/constants.h>
#include <it2s-tender/queue.h>
typedef enum MVER {
MVER_1,
MVER_2
} MVER_e;
enum ID_CHANGE_STAGE {
ID_CHANGE_INACTIVE,
ID_CHANGE_BLOCKED,
@ -59,9 +54,9 @@ typedef struct facilities {
void* apps_socket; /* alternative to tx queue, only used in rx/main thread */
struct {
enum MVER defaultv;
uint8_t defaultv;
bool handle_all;
} mver;
} pver;
// CA
lightship_t lightship;

2
src/infrastructure.c
View File

@ -578,7 +578,7 @@ void* infrastructure_service() {
pthread_mutex_unlock(&infrastructure->lock);
++sleep_count;
usleep(sleep4us);
itss_usleep(sleep4us);
}
return NULL;

61
src/requests.c
View File

@ -52,26 +52,6 @@ static void fwd_to_apps(uint8_t* msg, uint16_t msg_len, int its_msg_type, uint32
ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesIndication, fi);
}
static int pver2mver(uint8_t pver) {
switch (pver) {
case 1:
if (!facilities.mver.handle_all &&
facilities.mver.defaultv != MVER_1) {
return -1;
}
return MVER_1;
case 2:
if (!facilities.mver.handle_all &&
facilities.mver.defaultv != MVER_2) {
return -1;
}
return MVER_2;
default:
return -1;
}
}
int facilities_request_result_accepted(void *responder) {
int rv = 0;
@ -241,20 +221,20 @@ int facilities_request_single_message(void *responder, EIS_FacilitiesMessageRequ
ph->list.array[0] = calloc(1, sizeof(EI1_PathPoint_t));
itss_space_lock();
if (path_history[0]->alt != EI1_AltitudeValue_unavailable && epv.space.altitude != EI1_AltitudeValue_unavailable) {
ph->list.array[0]->pathPosition.deltaAltitude = path_history[0]->alt - epv.space.altitude;
if (path_history[0]->alt != EI1_AltitudeValue_unavailable && epv.space.data.altitude.value != EI1_AltitudeValue_unavailable) {
ph->list.array[0]->pathPosition.deltaAltitude = path_history[0]->alt - epv.space.data.altitude.value;
} else {
ph->list.array[0]->pathPosition.deltaAltitude = EI1_DeltaAltitude_unavailable;
}
if (path_history[0]->lat != EI1_Latitude_unavailable && epv.space.latitude != EI1_Latitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLatitude = path_history[0]->lat - epv.space.latitude;
if (path_history[0]->lat != EI1_Latitude_unavailable && epv.space.data.latitude.value != EI1_Latitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLatitude = path_history[0]->lat - epv.space.data.latitude.value;
} else {
ph->list.array[0]->pathPosition.deltaLatitude = EI1_DeltaLatitude_unavailable;
}
if (path_history[0]->lon != EI1_Longitude_unavailable && epv.space.longitude != EI1_Longitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLongitude = path_history[0]->lon - epv.space.longitude;
if (path_history[0]->lon != EI1_Longitude_unavailable && epv.space.data.longitude.value != EI1_Longitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLongitude = path_history[0]->lon - epv.space.data.longitude.value;
} else {
ph->list.array[0]->pathPosition.deltaLongitude = EI1_DeltaLongitude_unavailable;
}
@ -683,6 +663,12 @@ static int networking_packet_indication_btp(EIS_NetworkingPacketIndication_t* np
uint8_t buf[buf_len];
uint64_t id = 0;
// No message
if (npi->data.size == 0) {
return 1;
}
uint8_t pver = npi->data.buf[0];
// Parse message
@ -700,9 +686,15 @@ static int networking_packet_indication_btp(EIS_NetworkingPacketIndication_t* np
break;
case EIS_Port_denm:
its_msg_descriptor = &asn_DEF_EI1_DENM;
its_msg = calloc(1, sizeof(EI1_DENM_t));
its_msg_type = EI1_messageID_denm;
switch (pver) {
/* Only DENMv2 is supported (as in 1.3.1 - 2.1.1) */
/* Use by default CDDv2 */
case 2:
its_msg_descriptor = &asn_DEF_EI2_DENM;
its_msg = calloc(1, sizeof(EI2_DENM_t));
its_msg_type = EI2_MessageId_denm;
break;
}
break;
case EIS_Port_ivim:
@ -797,16 +789,7 @@ static int networking_packet_indication_btp(EIS_NetworkingPacketIndication_t* np
// Manage message
switch (bpi->destinationPort) {
case EIS_Port_cam:
switch (pver2mver(pver)) {
case MVER_2:
rv = check_cam_v2(bpi, its_msg, ssp, ssp_len);
break;
default:
log_debug("<- not processing CAMv%d", pver);
goto cleanup;
}
rv = check_cam(bpi, its_msg, ssp, ssp_len);
switch (rv) {
case CAM_OK:
fwd = true;

7
src/saem.c
View File

@ -4,7 +4,6 @@
#include "tpm.h"
#include <it2s-tender/time.h>
#include <it2s-tender/geodesy.h>
#include <it2s-tender/space.h>
#include <it2s-tender/recorder.h>
#include <it2s-tender/packet.h>
@ -381,8 +380,8 @@ void *sa_service() {
int32_t lat, lon;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
itss_space_unlock();
uint64_t now = itss_time_get();
@ -435,7 +434,7 @@ void *sa_service() {
tolling_tlsc_mgmt(facilities.tx_queue, &security_socket);
}
usleep(sleep_ms*1000);
itss_usleep(sleep_ms*1000);
}
ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingRequest, nr);

33
src/tpm.c
View File

@ -9,7 +9,6 @@
#include <it2s-tender/time.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
#include <it2s-tender/space.h>
#include <it2s-tender/geodesy.h>
#include <it2s-tender/recorder.h>
#include <it2s-tender/packet.h>
#include <stdint.h>
@ -28,12 +27,12 @@ int tpm_is_inside_zone(tolling_info_t* ti) {
int16_t heading;
itss_space_lock();
itss_space_get();
point[0] = epv.space.latitude/1.0e7;
point[1] = epv.space.longitude/1.0e7;
heading = epv.space.heading;
point[0] = epv.space.data.latitude.value/1.0e7;
point[1] = epv.space.data.longitude.value/1.0e7;
heading = epv.space.data.heading.value;
itss_space_unlock();
if (itss_is_inside_polygon(point, ti->zone.polygon, ti->zone.polygon_len)) {
if (it2s_geodesy_inside_polygon(point[0], point[1], ti->zone.polygon, ti->zone.polygon_len)) {
uint16_t da = abs((int16_t)heading - (int16_t)ti->asn->angle);
if (da <= 900 || da >= 2700) {
return 1;
@ -111,12 +110,12 @@ int tpm_pay(tolling_info_t* info, void** security_socket, uint8_t* neighbour, ui
// referencePosition
itss_space_lock();
itss_space_get();
tpm->tpm->referencePosition.altitude.altitudeValue = epv.space.altitude;
tpm->tpm->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
tpm->tpm->referencePosition.latitude = epv.space.latitude;
tpm->tpm->referencePosition.longitude = epv.space.longitude;
uint16_t lat_conf = epv.space.latitude_conf;
uint16_t lon_conf = epv.space.longitude_conf;
tpm->tpm->referencePosition.altitude.altitudeValue = epv.space.data.altitude.value;
tpm->tpm->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
tpm->tpm->referencePosition.latitude = epv.space.data.latitude.value;
tpm->tpm->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;
itss_space_unlock();
if (lat_conf > lon_conf) {
tpm->tpm->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
@ -708,12 +707,12 @@ static int rsu_handle_recv(EI1_TPM_t* tpm_rx, void** security_socket, uint8_t* n
// referencePosition
itss_space_lock();
itss_space_get();
tpm->tpm->referencePosition.altitude.altitudeValue = epv.space.altitude;
tpm->tpm->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
tpm->tpm->referencePosition.latitude = epv.space.latitude;
tpm->tpm->referencePosition.longitude = epv.space.longitude;
uint16_t lat_conf = epv.space.latitude_conf;
uint16_t lon_conf = epv.space.longitude_conf;
tpm->tpm->referencePosition.altitude.altitudeValue = epv.space.data.altitude.value;
tpm->tpm->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
tpm->tpm->referencePosition.latitude = epv.space.data.latitude.value;
tpm->tpm->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;
itss_space_unlock();
if (lat_conf > lon_conf) {
tpm->tpm->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;

304
src/vcm.c
View File

@ -1,15 +1,13 @@
#include "vcm.h"
#include "facilities.h"
#include <it2s-gnss.h>
#include <it2s-tender/epv.h>
#include <it2s-tender/space.h>
#include <it2s-tender/trajectory.h>
#include <it2s-tender/geodesy.h>
#include <it2s-tender/recorder.h>
#include <it2s-tender/packet.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/vcm/EI1_VCM.h>
#include <tgmath.h>
@ -21,26 +19,31 @@ static int do_paths_intersect(
itss_st_t* tB, int tB_len, uint16_t vB_length, uint16_t vB_width,
int* index
) {
/* TODO this can be made way more efficient */
double A1[2], A2[2], B1[2], B2[2];
double A[2][2], B[2][2];
uint64_t tsA, tsB;
for (int a = 0; a < tA_len-1; ++a) {
A1[0] = tA[a].latitude;
A1[1] = tA[a].longitude;
A2[0] = tA[a+1].latitude;
A2[1] = tA[a+1].longitude;
double A[2][2] = {
{tA[a].latitude, tA[a].longitude},
{tA[a+1].latitude, tA[a+1].longitude}
};
for (int b = 0; b < tB_len-1; ++b) {
if (tA[a].timestamp > tB[b].timestamp + 2000 ||
tA[a].timestamp < tB[b].timestamp - 2000) {
continue;
}
double B[2][2] = {
{tB[b].latitude, tB[b].longitude},
{tB[b+1].latitude, tB[b+1].longitude}
};
B1[0] = tB[b].latitude;
B1[1] = tB[b].longitude;
B2[0] = tB[b+1].latitude;
B2[1] = tB[b+1].longitude;
if (itss_do_segments_intersect(A1, A2, B1, B2)) {
if (it2s_geodesy_segments_intersect(&A, &B)) {
*index = a;
return 1;
}
@ -178,19 +181,20 @@ static void vcm_reject(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
GNSSDeltaPosition dpA[TRAJECTORY_MAX_LEN];
int dpA_len;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
heading = epv.space.data.heading.value;
heading_conf = epv.space.data.heading.confidence;
dpA_len = epv.space.data.ptraj_len;
memcpy(dpA, epv.space.data.ptraj, dpA_len * sizeof(GNSSDeltaPosition));
itss_space_unlock();
uint64_t now = itss_time_get();
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
ssize_t trajectoryA_len = 0;
EIS_NetworkingRequest_t* tr = NULL;
EIS_FacilitiesIndication_t* fi = NULL;
EI1_VCM_t* vcm_rep = calloc(1, sizeof(EI1_VCM_t));
@ -236,25 +240,15 @@ static void vcm_reject(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc_rep->vehicleWidth = facilities.vehicle.width;
itss_trajectory_lock();
trajectoryA_len = epv.trajectory.len;
memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
itss_trajectory_unlock();
trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon;
trajectoryA[0].timestamp = now;
++trajectoryA_len;
// Planned trajectory
mvc_rep->plannedTrajectory.list.count = trajectoryA_len - 1;
mvc_rep->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*);
mvc_rep->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
for (int i = 0; i < trajectoryA_len - 1; ++i) {
mvc_rep->plannedTrajectory.list.count = dpA_len;
mvc_rep->plannedTrajectory.list.size = dpA_len;
mvc_rep->plannedTrajectory.list.array = malloc(dpA_len * sizeof(void*));
for (int i = 0; i < dpA_len; ++i) {
mvc_rep->plannedTrajectory.list.array[i] = calloc(1, sizeof(EI1_STPoint_t));
mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
mvc_rep->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = dpA[i].deltaLatitude._0;
mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = dpA[i].deltaLongitude._0;
mvc_rep->plannedTrajectory.list.array[i]->deltaTime = dpA[i].deltaTime._0;
}
mvc_rep->negotiation = calloc(1, sizeof(EI1_CoordinationNegotiation_t));
@ -322,16 +316,22 @@ static bool commit() {
vcm_com->header.stationID = facilities.id.station_id;
pthread_mutex_unlock(&facilities.id.lock);
GNSSDeltaPosition dpA[TRAJECTORY_MAX_LEN];
int dpA_len;
int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
heading = epv.space.data.heading.value;
heading_conf = epv.space.data.heading.confidence;
dpA_len = epv.space.data.ptraj_len;
memcpy(dpA, epv.space.data.ptraj, dpA_len * sizeof(GNSSDeltaPosition));
itss_space_unlock();
vcm_com->vcm.currentPosition.latitude = lat;
vcm_com->vcm.currentPosition.longitude = lon;
@ -348,27 +348,14 @@ static bool commit() {
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc->vehicleWidth = facilities.vehicle.width;
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
uint16_t trajectoryA_len = 0;
itss_trajectory_lock();
trajectoryA_len = epv.trajectory.len;
memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
itss_trajectory_unlock();
trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon;
trajectoryA[0].timestamp = now;
++trajectoryA_len;
mvc->plannedTrajectory.list.count = trajectoryA_len - 1;
mvc->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*);
mvc->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
for (int i = 0; i < trajectoryA_len - 1; ++i) {
mvc->plannedTrajectory.list.count = dpA_len;
mvc->plannedTrajectory.list.size = dpA_len;
mvc->plannedTrajectory.list.array = malloc(dpA_len * sizeof(void*));
for (int i = 0; i < dpA_len; ++i) {
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(EI1_STPoint_t));
mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
mvc->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
mvc->plannedTrajectory.list.array[i]->deltaLatitude = dpA[i].deltaLatitude._0;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = dpA[i].deltaLongitude._0;
mvc->plannedTrajectory.list.array[i]->deltaTime = dpA[i].deltaTime._0;
}
mvc->negotiation = calloc(1, sizeof(EI1_CoordinationNegotiation_t));
@ -471,42 +458,36 @@ static int vcm_check_handle_request(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
ssize_t trajectoryA_len = 0;
GNSSDeltaPosition dpA[TRAJECTORY_MAX_LEN];
int dpA_len;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
heading = epv.space.data.heading.value;
heading_conf = epv.space.data.heading.confidence;
dpA_len = epv.space.data.ptraj_len;
memcpy(dpA, epv.space.data.ptraj, dpA_len * sizeof(GNSSDeltaPosition));
itss_space_unlock();
double dreq = itss_haversine(
double dreq = it2s_geodesy_haversine(
trj[h].latitude / 1.0e7, trj[h].longitude / 1.0e7,
vcm->vcm.currentPosition.latitude / 1.0e7, vcm->vcm.currentPosition.longitude / 1.0e7
);
double dego = itss_haversine(
double dego = it2s_geodesy_haversine(
trj[h].latitude / 1.0e7, trj[h].longitude / 1.0e7,
lat / 1.0e7, lon / 1.0e7
);
free(trj);
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.speed = calloc(1, sizeof(EIS_ManagementSpeedSet_t));
EIS_ManagementSpeedSet_t* mgss = mreq->choice.attributes.choice.set.speed;
mgss->rate = 5; /* km/h/s */
mgss->temporary = true; /* go back to original speed after a while */
mgss->type.present = EIS_ManagementSpeedSetType_PR_diff; /* differential change set */
mgss->type.choice.diff = (dreq > dego) ? 10 : -10; /* % */
asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_EIS_ManagementRequest, mreq, buf1, buf_len);
if (enc.encoded == -1) {
log_error("[vc] failed to encode MReq.speedSet (%s)", enc.failed_type->name);
}
itss_0send(coordination->mgmt_socket, buf1, enc.encoded);
if (itss_0recv_rt(&coordination->mgmt_socket, buf2, buf_len, buf1, enc.encoded, 500) == -1) {
log_error("[vc]-> MReq.speedSet ->[management] <TIMEOUT>");
}
ASN_STRUCT_FREE(asn_DEF_EIS_ManagementRequest, mreq);
itss_space_lock();
it2s_gnss_reqset_t reqset = {0};
reqset.speed.value = epv.space.data.speed.value + (dreq > dego ? 360 : -360) ; /* +- 10 km/h */
reqset.speed.present = 1;
itss_space_set(&reqset);
itss_space_unlock();
// Respond
EI1_VCM_t* vcm_rep = NULL;
@ -514,9 +495,6 @@ static int vcm_check_handle_request(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
EIS_FacilitiesIndication_t* fi = NULL;
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
ssize_t trajectoryA_len = 0;
neighbour->proposed = true;
neighbour->session.requested = true;
neighbour->session.nonce = request->nonce;
@ -573,15 +551,15 @@ static int vcm_check_handle_request(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc_rep->vehicleWidth = facilities.vehicle.width;
itss_trajectory_lock();
trajectoryA_len = epv.trajectory.len;
memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
itss_trajectory_unlock();
trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon;
trajectoryA[0].timestamp = now;
++trajectoryA_len;
for (int i = 0; i < dpA_len; ++i) {
trajectoryA[i+1].latitude = trajectoryA[i].latitude + dpA[i].deltaLatitude._0;
trajectoryA[i+1].longitude = trajectoryA[i].longitude + dpA[i].deltaLongitude._0;
trajectoryA[i+1].timestamp = trajectoryA[i].timestamp + dpA[i].deltaTime._0;
}
trajectoryA_len = dpA_len + 1;
// Planned trajectory
mvc_rep->plannedTrajectory.list.count = trajectoryA_len - 1;
@ -589,9 +567,9 @@ static int vcm_check_handle_request(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
mvc_rep->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
for (int i = 0; i < trajectoryA_len - 1; ++i) {
mvc_rep->plannedTrajectory.list.array[i] = calloc(1, sizeof(EI1_STPoint_t));
mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
mvc_rep->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = dpA[i].deltaLatitude._0;
mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = dpA[i].deltaLongitude._0;
mvc_rep->plannedTrajectory.list.array[i]->deltaTime = dpA[i].deltaTime._0;
}
// Accepted trajectory
@ -753,30 +731,33 @@ static int intersection_detected(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
uint16_t trajectoryA_len = 0;
GNSSDeltaPosition dpA[TRAJECTORY_MAX_LEN];
int dpA_len;
uint64_t now = itss_time_get();
int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
heading = epv.space.data.heading.value;
heading_conf = epv.space.data.heading.confidence;
dpA_len = epv.space.data.ptraj_len;
memcpy(dpA, epv.space.data.ptraj, dpA_len * sizeof(GNSSDeltaPosition));
itss_space_unlock();
itss_trajectory_lock();
trajectoryA_len = epv.trajectory.len;
memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
itss_trajectory_unlock();
trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon;
trajectoryA[0].timestamp = now;
++trajectoryA_len;
for (int i = 0; i < dpA_len; ++i) {
trajectoryA[i+1].latitude = trajectoryA[i].latitude + dpA[i].deltaLatitude._0;
trajectoryA[i+1].longitude = trajectoryA[i].longitude + dpA[i].deltaLongitude._0;
trajectoryA[i+1].timestamp = trajectoryA[i].timestamp + dpA[i].deltaTime._0;
}
trajectoryA_len = dpA_len + 1;
// Initiate conflict resolution
vcm_req = calloc(1, sizeof(EI1_VCM_t));
@ -789,7 +770,7 @@ static int intersection_detected(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
vcm_req->vcm.currentPosition.latitude = lat;
vcm_req->vcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&vcm_req->vcm.currentPosition.timestamp, now);
asn_uint642INTEGER(&vcm_req->vcm.currentPosition.timestamp, now);
/*
if (coordination->chain.enabled && coordination->chain.id) {
@ -821,14 +802,14 @@ static int intersection_detected(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
mvc->vehicleWidth = facilities.vehicle.width;
// Planned trajectory
mvc->plannedTrajectory.list.count = trajectoryA_len - 1;
mvc->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*);
mvc->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
for (int i = 0; i < trajectoryA_len - 1; ++i) {
mvc->plannedTrajectory.list.count = dpA_len;
mvc->plannedTrajectory.list.size = dpA_len;
mvc->plannedTrajectory.list.array = malloc(dpA_len * sizeof(void*));
for (int i = 0; i < dpA_len; ++i) {
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(EI1_STPoint_t));
mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
mvc->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
mvc->plannedTrajectory.list.array[i]->deltaLatitude = dpA[i].deltaLatitude._0;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = dpA[i].deltaLongitude._0;
mvc->plannedTrajectory.list.array[i]->deltaTime = dpA[i].deltaTime._0;
}
// Desired trajectory
@ -843,14 +824,14 @@ static int intersection_detected(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
mvc->negotiation->choice.request.nonce = rand();
coordination->session.nonce = mvc->negotiation->choice.request.nonce;
pt->trajectory.list.count = trajectoryA_len - 1;
pt->trajectory.list.size = sizeof(void*) * (trajectoryA_len - 1);
pt->trajectory.list.array = malloc(sizeof(void*) * (trajectoryA_len - 1));
for (int i = 0; i < trajectoryA_len - 1; ++i) {
pt->trajectory.list.count = dpA_len;
pt->trajectory.list.size = dpA_len;
pt->trajectory.list.array = malloc(sizeof(void*) * dpA_len);
for (int i = 0; i < dpA_len; ++i) {
pt->trajectory.list.array[i] = calloc(1, sizeof(EI1_STPoint_t));
pt->trajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
pt->trajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
pt->trajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
pt->trajectory.list.array[i]->deltaLatitude = dpA[i].deltaLatitude._0;
pt->trajectory.list.array[i]->deltaLongitude = dpA[i].deltaLongitude._0;
pt->trajectory.list.array[i]->deltaTime = dpA[i].deltaTime._0;
}
pt->offer = 5;
@ -923,25 +904,29 @@ static void intersection_check(EI1_VCM_t* vcm, mc_neighbour_s* neighbour) {
itss_st_t trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */
uint16_t trajectoryA_len = 0;
uint16_t trajectoryB_len = 0;
GNSSDeltaPosition dpA[TRAJECTORY_MAX_LEN];
int dpA_len;
int32_t lat, lon;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
dpA_len = epv.space.data.ptraj_len;
memcpy(dpA, epv.space.data.ptraj, dpA_len * sizeof(GNSSDeltaPosition));
itss_space_unlock();
uint64_t now = itss_time_get();
itss_trajectory_lock();
trajectoryA_len = epv.trajectory.len;
memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
itss_trajectory_unlock();
trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon;
trajectoryA[0].timestamp = now;
++trajectoryA_len;
for (int i = 0; i < dpA_len; ++i) {
trajectoryA[i+1].latitude = trajectoryA[i].latitude + dpA[i].deltaLatitude._0;
trajectoryA[i+1].longitude = trajectoryA[i].longitude + dpA[i].deltaLongitude._0;
trajectoryA[i+1].timestamp = trajectoryA[i].timestamp + dpA[i].deltaTime._0;
}
trajectoryA_len = dpA_len + 1;
trajectoryB[0].latitude = vcm->vcm.currentPosition.latitude;
trajectoryB[0].longitude = vcm->vcm.currentPosition.longitude;
@ -1076,8 +1061,6 @@ static int mk_vcm() {
coordination_t* coordination = &facilities.coordination;
itss_st_t trajectory[TRAJECTORY_MAX_LEN];
EI1_VCM_t* vcm = calloc(1, sizeof(EI1_VCM_t));
vcm->header.messageID = 43;
@ -1091,19 +1074,18 @@ static int mk_vcm() {
uint16_t heading;
uint8_t heading_conf;
uint16_t trajectory_len = 0;
GNSSDeltaPosition dpA[TRAJECTORY_MAX_LEN];
int dpA_len;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
lat = epv.space.data.latitude.value;
lon = epv.space.data.longitude.value;
heading = epv.space.data.heading.value;
heading_conf = epv.space.data.heading.confidence;
dpA_len = epv.space.data.ptraj_len;
memcpy(dpA, epv.space.data.ptraj, dpA_len * sizeof(GNSSDeltaPosition));
itss_space_unlock();
itss_trajectory_lock();
trajectory_len = epv.trajectory.len;
memcpy(trajectory, epv.trajectory.path, trajectory_len * sizeof(itss_st_t));
itss_trajectory_unlock();
vcm->vcm.currentPosition.latitude = lat;
vcm->vcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&vcm->vcm.currentPosition.timestamp, now);
@ -1141,18 +1123,14 @@ static int mk_vcm() {
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc->vehicleWidth = facilities.vehicle.width;
mvc->plannedTrajectory.list.count = trajectory_len;
mvc->plannedTrajectory.list.size = trajectory_len * sizeof(void*);
mvc->plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*));
mvc->plannedTrajectory.list.array[0] = calloc(1, sizeof(EI1_STPoint_t));
mvc->plannedTrajectory.list.array[0]->deltaLatitude = trajectory[0].latitude - lat;
mvc->plannedTrajectory.list.array[0]->deltaLongitude = trajectory[0].longitude - lon;
mvc->plannedTrajectory.list.array[0]->deltaTime = trajectory[0].timestamp - now;
for (int i = 1; i < trajectory_len; ++i) {
mvc->plannedTrajectory.list.count = dpA_len;
mvc->plannedTrajectory.list.size = dpA_len * sizeof(void*);
mvc->plannedTrajectory.list.array = malloc(dpA_len * sizeof(void*));
for (int i = 0; i < dpA_len; ++i) {
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(EI1_STPoint_t));
mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectory[i].latitude - trajectory[i-1].latitude;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectory[i].longitude - trajectory[i-1].longitude;
mvc->plannedTrajectory.list.array[i]->deltaTime = trajectory[i].timestamp - trajectory[i-1].timestamp;
mvc->plannedTrajectory.list.array[i]->deltaLatitude = dpA[i].deltaLatitude._0;
mvc->plannedTrajectory.list.array[i]->deltaLongitude = dpA[i].deltaLongitude._0;
mvc->plannedTrajectory.list.array[i]->deltaTime = dpA[i].deltaTime._0;
}
}
@ -1178,23 +1156,23 @@ static bool vcm_timer_check(coordination_t* coordination, uint64_t now) {
if (facilities.station_type != EI1_StationType_roadSideUnit) { // Vehicle
itss_trajectory_lock();
itss_space_lock();
if (epv.trajectory.len > 2) {
if (epv.space.data.ptraj_len > 2) {
double c_sum = 0;
for (int i = 0; i < epv.trajectory.len - 2; ++i) {
double d1 = itss_haversine(
epv.trajectory.path[i+1].latitude / 1.0e7,
epv.trajectory.path[i+1].longitude / 1.0e7,
epv.trajectory.path[i].latitude / 1.0e7,
epv.trajectory.path[i].longitude / 1.0e7);
for (int i = 0; i < epv.space.data.ptraj_len - 2; ++i) {
double d1 = it2s_geodesy_haversine(
epv.space.data.ptraj[i].deltaLatitude._0 / 1.0e7,
epv.space.data.ptraj[i].deltaLongitude._0 / 1.0e7,
0.0,
0.0);
double t1 = atan2(
epv.trajectory.path[i+1].latitude - epv.trajectory.path[i].latitude ,
epv.trajectory.path[i+1].longitude - epv.trajectory.path[i].longitude);
epv.space.data.ptraj[i].deltaLatitude._0,
epv.space.data.ptraj[i].deltaLongitude._0);
double t2 = atan2(
epv.trajectory.path[i+2].latitude - epv.trajectory.path[i].latitude ,
epv.trajectory.path[i+2].longitude - epv.trajectory.path[i].longitude);
epv.space.data.ptraj[i+1].deltaLatitude._0 + epv.space.data.ptraj[i].deltaLatitude._0,
epv.space.data.ptraj[i+1].deltaLongitude._0 + epv.space.data.ptraj[i].deltaLongitude._0);
c_sum += fabs(sin(t2-t1)*d1);
}
@ -1205,7 +1183,7 @@ static bool vcm_timer_check(coordination_t* coordination, uint64_t now) {
}
}
itss_trajectory_unlock(epv);
itss_space_unlock(epv);
}
return send;
@ -1231,7 +1209,7 @@ void* vc_service() {
}
pthread_mutex_unlock(&coordination->lock);
usleep(50 * 1000);
itss_usleep(50 * 1000);
}
itss_0close(coordination->mgmt_socket);

10
src/vcm.h
View File

@ -3,7 +3,6 @@
#include <stdbool.h>
#include <it2s-asn/etsi-its-v1/vcm/EI1_VCM.h>
#include <it2s-tender/geodesy.h>
#include <it2s-tender/epv.h>
#define MC_MAX_NEIGHBOURS 256
@ -12,6 +11,7 @@
#define MC_AFF_STATIONS_N_MAX 7
#define MC_HASH_LINK_LEN 32
#define MC_SESSIONS_N_MAX 16
#define TRAJECTORY_MAX_LEN 16
typedef enum MC_PROTOCOL {
MC_PROTOCOL_VCM_RR,
@ -19,6 +19,13 @@ typedef enum MC_PROTOCOL {
MC_PROTOCOL_VCM_RRAC
} MC_PROTOCOL_e;
typedef struct itss_st {
int32_t latitude;
int32_t longitude;
int32_t altitude;
uint64_t timestamp;
} itss_st_t;
typedef struct mc_neighbour {
uint32_t station_id;
uint16_t length;
@ -79,7 +86,6 @@ typedef struct coordination {
uint64_t id;
uint8_t links[MC_AFF_STATIONS_N_MAX + 1][MC_HASH_LINK_LEN];
uint8_t n_links;
itss_region_t region;
} chain;
} coordination_t;