From c75624d8cad115b3dcb334ce74e66794e8367e22 Mon Sep 17 00:00:00 2001 From: emanuel Date: Mon, 5 Jul 2021 18:26:59 +0100 Subject: [PATCH] Add confidence values to GPS coords, calculate CAM pos confidence --- src/cam.c | 15 ++++++++++++--- src/config.c | 2 ++ src/facilities.c | 2 ++ 3 files changed, 16 insertions(+), 3 deletions(-) diff --git a/src/cam.c b/src/cam.c index 708b50e..f0d9f77 100644 --- a/src/cam.c +++ b/src/cam.c @@ -116,9 +116,8 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) // Set GPS coordinates bc->referencePosition.latitude = facilities->epv.space.latitude; bc->referencePosition.longitude = facilities->epv.space.longitude; - bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; - bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; - bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; + uint16_t lat_conf = facilities->epv.space.latitude_conf; + uint16_t lon_conf = facilities->epv.space.longitude_conf; cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; @@ -133,6 +132,16 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) 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; diff --git a/src/config.c b/src/config.c index 2f9b3f9..982bb7f 100644 --- a/src/config.c +++ b/src/config.c @@ -342,7 +342,9 @@ int facilities_config(void* facilities_s) { mrep->attributes->clockOffset && mrep->attributes->gpsType) { facilities->epv.space.latitude = mrep->attributes->coordinates->latitude; + facilities->epv.space.latitude_conf = mrep->attributes->coordinates->latitudeConfidence; facilities->epv.space.longitude = mrep->attributes->coordinates->longitude; + facilities->epv.space.longitude_conf = mrep->attributes->coordinates->longitudeConfidence; facilities->epv.space.altitude = mrep->attributes->altitude->altitudeValue; facilities->epv.space.altitude_conf = mrep->attributes->altitude->altitudeConfidence; facilities->epv.space.heading = mrep->attributes->heading->headingValue; diff --git a/src/facilities.c b/src/facilities.c index 9a18dc7..e20b3df 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -402,7 +402,9 @@ static int management_indication(facilities_t* facilities, void* responder, uint if (mi->present == ManagementIndication_PR_attributes) { it2s_tender_lock_space(&facilities->epv); facilities->epv.space.latitude = mi->choice.attributes.coordinates.latitude; + facilities->epv.space.latitude_conf = mi->choice.attributes.coordinates.latitudeConfidence; facilities->epv.space.longitude = mi->choice.attributes.coordinates.longitude; + facilities->epv.space.longitude_conf = mi->choice.attributes.coordinates.longitudeConfidence; facilities->epv.space.speed = mi->choice.attributes.speed.speedValue; facilities->epv.space.speed_conf = mi->choice.attributes.speed.speedConfidence; facilities->epv.space.heading = mi->choice.attributes.heading.headingValue;