diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 1daffd5..7798adb 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -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 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f907f06..c8294d8 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -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 diff --git a/src/cam.c b/src/cam.c index 805478a..60bc9fd 100644 --- a/src/cam.c +++ b/src/cam.c @@ -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; } diff --git a/src/cam.h b/src/cam.h index 366f022..441df94 100644 --- a/src/cam.h +++ b/src/cam.h @@ -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 diff --git a/src/config.c b/src/config.c index 381cf98..fc10e58 100644 --- a/src/config.c +++ b/src/config.c @@ -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)); diff --git a/src/cpm.c b/src/cpm.c index 729457c..81b2544 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -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 */ diff --git a/src/denm.c b/src/denm.c index b28c1bc..5763ddb 100644 --- a/src/denm.c +++ b/src/denm.c @@ -6,6 +6,8 @@ #include #include +#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; diff --git a/src/denm.h b/src/denm.h index b7de1fe..30a391c 100644 --- a/src/denm.h +++ b/src/denm.h @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -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(); diff --git a/src/evm.c b/src/evm.c index b87fce2..73125b7 100644 --- a/src/evm.c +++ b/src/evm.c @@ -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) { diff --git a/src/facilities.c b/src/facilities.c index 0b2d5ee..df3f743 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -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(); diff --git a/src/facilities.h b/src/facilities.h index 62c6a75..4772e9f 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -21,11 +21,6 @@ #include #include -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; diff --git a/src/infrastructure.c b/src/infrastructure.c index c6870eb..dc3dde5 100644 --- a/src/infrastructure.c +++ b/src/infrastructure.c @@ -578,7 +578,7 @@ void* infrastructure_service() { pthread_mutex_unlock(&infrastructure->lock); ++sleep_count; - usleep(sleep4us); + itss_usleep(sleep4us); } return NULL; diff --git a/src/requests.c b/src/requests.c index ed0e801..264045d 100644 --- a/src/requests.c +++ b/src/requests.c @@ -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; diff --git a/src/saem.c b/src/saem.c index d163e02..647ca73 100644 --- a/src/saem.c +++ b/src/saem.c @@ -4,7 +4,6 @@ #include "tpm.h" #include -#include #include #include #include @@ -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); diff --git a/src/tpm.c b/src/tpm.c index 1b8c37f..b49642f 100644 --- a/src/tpm.c +++ b/src/tpm.c @@ -9,7 +9,6 @@ #include #include #include -#include #include #include #include @@ -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; diff --git a/src/vcm.c b/src/vcm.c index b0dc9a4..780b7c1 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -1,15 +1,13 @@ #include "vcm.h" #include "facilities.h" +#include #include #include -#include -#include #include #include #include #include -#include #include #include @@ -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] "); - } - 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); diff --git a/src/vcm.h b/src/vcm.h index 843c855..5b34f48 100644 --- a/src/vcm.h +++ b/src/vcm.h @@ -3,7 +3,6 @@ #include #include -#include #include #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;