From d6a6f0f1fd2b58e778e8f54bdfe8b09b530b424a Mon Sep 17 00:00:00 2001 From: emanuel Date: Thu, 27 Jan 2022 13:34:28 +0000 Subject: [PATCH] Polygonal tolling zones --- src/config.c | 7 ++-- src/saem.c | 17 +++----- src/tpm.c | 108 ++++++++++++++++++++------------------------------- src/tpm.h | 16 +++++++- 4 files changed, 65 insertions(+), 83 deletions(-) diff --git a/src/config.c b/src/config.c index 0302928..fe47b8d 100644 --- a/src/config.c +++ b/src/config.c @@ -396,12 +396,11 @@ int facilities_config(void* facilities_s) { asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_TollingPaymentInfo, (void**) &ti, ti_xml, size); if (!dec.code) { - facilities->tolling.infos.z[i] = ti; + facilities->tolling.infos.z[i] = calloc(1, sizeof(tolling_info_s)); + facilities->tolling.infos.z[i]->asn = ti; ++facilities->tolling.infos.length; ++i; - syslog_debug("[facilities] [config] loaded tolling info @ (%ld, %ld) with %d payment option(s)", - ti->zone.latitude, - ti->zone.longitude, + syslog_debug("[facilities] [config] loaded tolling info with %d payment option(s)", (ti->fiatInfos ? ti->fiatInfos->list.count : 0) + (ti->ledgerInfos ? ti->ledgerInfos->list.count : 0) ); } else { diff --git a/src/saem.c b/src/saem.c index b087c72..d454a6d 100644 --- a/src/saem.c +++ b/src/saem.c @@ -79,18 +79,20 @@ SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem, uint8_t* ne ); if (facilities->station_type != 15 && facilities->tolling.infos.length < TOLLING_INFOS_MAX_LENGTH) { + TollingPaymentInfo_t* tpi = NULL; asn_dec_rval_t dec = uper_decode_complete( NULL, &asn_DEF_TollingPaymentInfo, - (void**) &facilities->tolling.infos.z[facilities->tolling.infos.length], + (void**) &tpi, si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size ); if (!dec.code) { + facilities->tolling.infos.z[facilities->tolling.infos.length] = tolling_info_new(&facilities->epv, tpi); bulletin->to_consume[bulletin->to_provide_len]->info.internal_p = facilities->tolling.infos.z[facilities->tolling.infos.length]; ++facilities->tolling.infos.length; } else { - ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, facilities->tolling.infos.z[facilities->tolling.infos.length]); + ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, tpi); } } break; @@ -349,16 +351,7 @@ void *sa_service(void *fc) { !bulletin->to_consume[a]->n_trigger && facilities->station_type != 15) { - if (!it2s_tender_is_inside_rectangle( - lat/1e07, - lon/1e07, - ((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.latitude/1e07, - ((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.longitude/1e07, - ((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.a/10.0, - ((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.b/10.0, - ((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.angle/10.0, - DCM_VINCENTY - )) { + if (!tpm_is_inside_zone(facilities, (tolling_info_s*) bulletin->to_consume[a]->info.internal_p)) { continue; } diff --git a/src/tpm.c b/src/tpm.c index d496784..d8f2259 100644 --- a/src/tpm.c +++ b/src/tpm.c @@ -6,9 +6,28 @@ #include #include #include +#include #include #include +int tpm_is_inside_zone(void* fc, tolling_info_s* ti) { + int rv = 0; + + facilities_t* facilities = (facilities_t*) fc; + double point[2]; + it2s_tender_lock_space(&facilities->epv); + it2s_tender_get_space(&facilities->epv); + point[0] = facilities->epv.space.latitude/1.0e7; + point[1] = facilities->epv.space.longitude/1.0e7; + it2s_tender_unlock_space(&facilities->epv); + + if (it2s_tender_is_inside_polygon(point, ti->zone.polygon, ti->zone.polygon_len)) { + return 1; + } + + return 0; +} + int tpm_pay(void* fc, uint8_t* neighbour) { int rv = 0; @@ -44,55 +63,6 @@ int tpm_pay(void* fc, uint8_t* neighbour) { tpm->header.stationID = facilities->id.station_id; pthread_mutex_unlock(&facilities->id.lock); - BasicContainer_t* bc = &tpm->tpm.camParameters.basicContainer; - tpm->tpm.camParameters.basicContainer.stationType = facilities->station_type; - - it2s_tender_lock_space(&facilities->epv); - it2s_tender_get_space(&facilities->epv); - bc->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude; - bc->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf; - - // Set GPS coordinates - bc->referencePosition.latitude = facilities->epv.space.latitude; - bc->referencePosition.longitude = facilities->epv.space.longitude; - uint16_t lat_conf = facilities->epv.space.latitude_conf; - uint16_t lon_conf = facilities->epv.space.longitude_conf; - - tpm->tpm.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; - BasicVehicleContainerHighFrequency_t* bvc_hf = &tpm->tpm.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; - - // Set speed - bvc_hf->speed.speedValue = facilities->epv.space.speed; - bvc_hf->speed.speedConfidence = facilities->epv.space.speed_conf; - - // Set heading - bvc_hf->heading.headingValue = facilities->epv.space.heading; - bvc_hf->heading.headingConfidence = facilities->epv.space.heading_conf; - - it2s_tender_unlock_space(&facilities->epv); - - if (lat_conf > lon_conf) { - bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf; - bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf; - bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 0; - } else { - bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf; - bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf; - bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 900; - } - - bvc_hf->vehicleWidth = facilities->vehicle.width; - bvc_hf->vehicleLength.vehicleLengthValue = facilities->vehicle.length; - bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; - bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; - bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; - - bvc_hf->driveDirection = DriveDirection_unavailable; - bvc_hf->curvature.curvatureValue = CurvatureValue_unavailable; - bvc_hf->curvature.curvatureConfidence = CurvatureConfidence_unavailable; - bvc_hf->yawRate.yawRateValue = YawRateValue_unavailable; - bvc_hf->yawRate.yawRateConfidence = YawRateConfidence_unavailable; - asn_ulong2INTEGER(&tpm->tpm.timestamp, it2s_tender_get_clock(&facilities->epv)); tpm->tpm.tollingFlow.present = TollingFlow_PR_request; @@ -290,23 +260,6 @@ static void rsu_handle_recv(facilities_t* facilities, TPM_t* tpm_rx, uint8_t* ne asn_ulong2INTEGER(&tpm->tpm.timestamp, it2s_tender_get_clock(&facilities->epv)); - BasicContainer_t* bc = &tpm->tpm.camParameters.basicContainer; - tpm->tpm.camParameters.basicContainer.stationType = facilities->station_type; - - it2s_tender_lock_space(&facilities->epv); - it2s_tender_get_space(&facilities->epv); - bc->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude; - bc->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf; - - // Set GPS coordinates - bc->referencePosition.latitude = facilities->epv.space.latitude; - bc->referencePosition.longitude = facilities->epv.space.longitude; - uint16_t lat_conf = facilities->epv.space.latitude_conf; - uint16_t lon_conf = facilities->epv.space.longitude_conf; - - tpm->tpm.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency; - it2s_tender_unlock_space(&facilities->epv); - tpm->tpm.tollingFlow.present = TollingFlow_PR_reply; tpm->tpm.tollingFlow.choice.reply.clientId = req->clientId; tpm->tpm.tollingFlow.choice.reply.transactionNonce = req->transactionNonce; @@ -538,3 +491,26 @@ int tolling_init(tolling_s* tolling, void* zmq_ctx, char* security_address) { return 0; } + +tolling_info_s* tolling_info_new(it2s_tender_epv_t* epv, TollingPaymentInfo_t* tpi) { + tolling_info_s* ti = calloc(1, sizeof(tolling_info_s)); + + ti->timestamp = it2s_tender_get_clock(epv); + ti->asn = tpi; + + ti->zone.polygon_len = tpi->zone.list.count; + ti->zone.polygon = malloc(tpi->zone.list.count * sizeof(double[2])); + + for (int i = 0; i < tpi->zone.list.count; ++i) { + ti->zone.polygon[i][0] = tpi->zone.list.array[i]->latitude / 1.0e7; + ti->zone.polygon[i][1] = tpi->zone.list.array[i]->longitude / 1.0e7; + } + + return ti; +} + +void tolling_info_free(tolling_info_s* ti) { + ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, ti->asn); + free(ti->zone.polygon); + free(ti); +} diff --git a/src/tpm.h b/src/tpm.h index 84a4987..6213eb6 100644 --- a/src/tpm.h +++ b/src/tpm.h @@ -12,6 +12,15 @@ typedef enum TOLLING_PROTOCOL { TOLLING_PROTOCOL_TLS } TOLLING_PROTOCOL_e; +typedef struct tolling_info { + uint64_t timestamp; + struct { + double (*polygon)[2]; /* (latitude, longitude) */ + size_t polygon_len; + } zone; + TollingPaymentInfo_t* asn; +} tolling_info_s; + typedef struct tolling { bool enabled; TOLLING_PROTOCOL_e protocol; @@ -24,7 +33,7 @@ typedef struct tolling { uint64_t tz; struct { - TollingPaymentInfo_t* z[TOLLING_INFOS_MAX_LENGTH]; + tolling_info_s* z[TOLLING_INFOS_MAX_LENGTH]; uint8_t length; } infos; @@ -36,3 +45,8 @@ typedef struct tolling { int tolling_init(tolling_s* tolling, void* zmq_ctx, char* security_address); int tpm_pay(void* fc, uint8_t* neighbour); int tpm_recv(void* fc, TPM_t* tpm_rx, uint8_t* neighbour); +int tpm_is_inside_zone(void* fc, tolling_info_s* ti); + +tolling_info_s* tolling_info_new(it2s_tender_epv_t* epv, TollingPaymentInfo_t* tpi); +void tolling_info_free(tolling_info_s* ti); +